restructuring

embedded/lib/Controller.hpp

@@ -0,0 +1,99 @@
+#ifndef CONTROLLER_HPP
+#define CONTROLLER_HPP
+
+#include <stdint.h>
+#include <avr/pgmspace.h>
+#include "IndSensorMap.hpp"
+
+// ── Pin Mapping ──────────────────────────────────────────────
+#define dirBL 2
+#define pwmBL 3
+#define dirBR 4
+#define pwmBR 10
+#define pwmFL 11
+#define dirFL 7
+#define dirFR 8
+#define pwmFR 9
+
+// ── Output Cap ───────────────────────────────────────────────
+#define CAP 250   // Max PWM magnitude (0-255 Arduino range)
+
+// ── PID Gains (single set per loop — matches simulation) ────
+typedef struct PIDGains {
+  float kp;
+  float ki;
+  float kd;
+} PIDGains;
+
+// ── 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;
+
+// ── 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
+
+// ── 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;        // Any sensor out-of-range
+  bool outputOn;   // Enable/disable output
+
+  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();
+
+  // Runtime tuning
+  void updateHeightPID(PIDGains gains);
+  void updateRollPID(PIDGains gains);
+  void updatePitchPID(PIDGains gains);
+  void updateReference(float avgReference);
+  void setFeedforward(bool enabled);
+
+private:
+  int16_t pidCompute(PIDGains& gains, PIDState& state, float maxOutput);
+  int16_t feedforward(float gapMM);
+
+  IndSensor& Left;
+  IndSensor& Right;
+  IndSensor& Front;
+  IndSensor& Back;
+
+  PIDGains heightGains;
+  PIDGains rollGains;
+  PIDGains pitchGains;
+
+  PIDState heightErr;
+  PIDState rollErr;
+  PIDState pitchErr;
+
+  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;
+};
+
+#endif // CONTROLLER_HPP