Loaded new mass and inertial characteristics into sim, ported sim arch into arduino code

lib/Controller.hpp

@@ -2,103 +2,98 @@
 #define CONTROLLER_HPP
 
 #include <stdint.h>
+#include <avr/pgmspace.h>
 #include "IndSensorMap.hpp"
 
-// PIN MAPPING
-#define dirFR 2
-#define pwmFR 3
+// ── Pin Mapping ──────────────────────────────────────────────
+#define dirBL 2
+#define pwmBL 3
 #define dirBR 4
 #define pwmBR 10
 #define pwmFL 11
 #define dirFL 7
-#define dirBL 8
-#define pwmBL 9
+#define dirFR 8
+#define pwmFR 9
 
-#define CAP 250
+// ── Output Cap ───────────────────────────────────────────────
+#define CAP 250   // Max PWM magnitude (0-255 Arduino range)
 
-typedef struct Constants {
+// ── PID Gains (single set per loop — matches simulation) ────
+typedef struct PIDGains {
   float kp;
   float ki;
   float kd;
-} Constants;
+} PIDGains;
 
-typedef struct K_MAP {
-  Constants repelling;
-  Constants attracting;
-} K_MAP;
+// ── PID Error State ──────────────────────────────────────────
+typedef struct PIDState {
+  float e;        // Current error
+  float eDiff;    // Derivative of error (e[k] - e[k-1])
+  float eInt;     // Integral of error (accumulated)
+} PIDState;
 
-typedef struct FullConsts {
-  K_MAP avg;
-  K_MAP lColl; // repelling is applied to attracting and vice versa for the Right and Back collectives.
-  K_MAP fColl;
-} FullConsts;
+// ── Feedforward LUT (PROGMEM) ────────────────────────────────
+// Generated by gen_ff_lut.py from MaglevPredictor model
+// Pod mass: 9.4 kg, Coil R: 1.1Ω, V_supply: 12V
+// Positive = repelling, Negative = attracting
+// Beyond ~16mm: clamped to -CAP (no equilibrium exists)
+#define FF_LUT_SIZE 64
+#define FF_GAP_MIN  3.0f
+#define FF_GAP_MAX  20.0f
+#define FF_GAP_STEP 0.269841f
 
-typedef struct Errors {
-  float e;
-  float eDiff;
-  float eInt;
-} Errors;
+// ── Geometry (mm, matching simulation) ───────────────────────
+// Sensor-to-sensor distances for angle computation
+#define Y_DISTANCE_MM 101.6f   // Left↔Right sensor spacing (mm)
+#define X_DISTANCE_MM 251.8f   // Front↔Back sensor spacing (mm)
 
+// ── Controller Class ─────────────────────────────────────────
 class FullController {
-  public:
-    bool oor;
-    bool outputOn;
+public:
+  bool oor;        // Any sensor out-of-range
+  bool outputOn;   // Enable/disable output
 
-    FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b,
-      FullConsts fullConsts, float avgRef, float lrDiffRef, float fbDiffRef)
-      : Left(l), Right(r), Front(f), Back(b), AvgRef(avgRef), LRDiffRef(lrDiffRef),
-      FBDiffRef(fbDiffRef), avgConsts(fullConsts.avg), LConsts(fullConsts.lColl),
-      FConsts(fullConsts.fColl), avgError({0,0,0}), LRDiffErr({0,0,0}), 
-      FBDiffErr({0,0,0}), oor(false), outputOn(false) {}
+  FullController(IndSensor& l, IndSensor& r, IndSensor& f, IndSensor& b,
+                 PIDGains heightGains, PIDGains rollGains, PIDGains pitchGains,
+                 float avgRef, bool useFeedforward);
 
-    void update();
-    void zeroPWMs();
-    void sendOutputs();
-    void report();
-    
-    // PID tuning methods
-    void updateAvgPID(Constants repel, Constants attract);
-    void updateLRPID(Constants down, Constants up);
-    void updateFBPID(Constants down, Constants up);
-    
-    // Reference update methods
-    void updateReferences(float avgReference, float lrDiffReference, float fbDiffReference);
+  void update();
+  void zeroPWMs();
+  void sendOutputs();
+  void report();
 
-  private:
-    void avgControl();
-    void LRControl();
-    void FBControl();
-    int16_t pwmFunc(K_MAP consts, Errors errs);
+  // Runtime tuning
+  void updateHeightPID(PIDGains gains);
+  void updateRollPID(PIDGains gains);
+  void updatePitchPID(PIDGains gains);
+  void updateReference(float avgReference);
+  void setFeedforward(bool enabled);
 
-    IndSensor& Front;
-    IndSensor& Back;
-    IndSensor& Right;
-    IndSensor& Left;
+private:
+  int16_t pidCompute(PIDGains& gains, PIDState& state, float maxOutput);
+  int16_t feedforward(float gapMM);
 
-    K_MAP avgConsts;
-    K_MAP LConsts;
-    K_MAP FConsts;
+  IndSensor& Left;
+  IndSensor& Right;
+  IndSensor& Front;
+  IndSensor& Back;
 
-    Errors avgError;
-    Errors LRDiffErr;
-    Errors FBDiffErr;
+  PIDGains heightGains;
+  PIDGains rollGains;
+  PIDGains pitchGains;
 
-    float AvgRef;
-    float LRDiffRef;
-    float FBDiffRef;
-    float avg;
+  PIDState heightErr;
+  PIDState rollErr;
+  PIDState pitchErr;
 
-    int16_t avgPWM;
-    int16_t LDiffPWM;
-    int16_t RDiffPWM;
-    int16_t FDiffPWM;
-    int16_t BDiffPWM;
-    // Initially, I was going to make the Right and Back just the negatives,
-    // but having separate control functions running for each of these outputs might prove useful.
+  float AvgRef;     // Target gap height (mm)
+  float avg;        // Current average gap (mm)
+  bool  ffEnabled;  // Feedforward on/off
 
-    int16_t FLPWM;
-    int16_t BLPWM;
-    int16_t FRPWM;
-    int16_t BRPWM;
+  int16_t FLPWM;
+  int16_t BLPWM;
+  int16_t FRPWM;
+  int16_t BRPWM;
 };
+
 #endif // CONTROLLER_HPP