heave control + plotter

embedded/lib/HeaveController.hpp

@@ -0,0 +1,87 @@
+#ifndef HEAVE_CONTROLLER_HPP
+#define HEAVE_CONTROLLER_HPP
+
+#include <stdint.h>
+#include "IndSensorLUT.hpp"
+
+// ── Pin Mapping (mirrors Controller.hpp) ─────────────────────
+#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 HEAVE_CAP 250
+
+// ── Feedforward LUT (gap mm → equilibrium PWM) ───────────────
+// Source: FF_PWM_LUT in Controller.cpp (pod 9.4 kg, R 1.1 Ω, V 12 V).
+// Positive = repel, negative = attract. Lives in PROGMEM.
+#define HEAVE_FF_LUT_SIZE 64
+#define HEAVE_FF_GAP_MIN  3.0f
+#define HEAVE_FF_GAP_MAX  20.0f
+#define HEAVE_FF_GAP_STEP 0.269841f
+
+// ── PID Gains / State ────────────────────────────────────────
+typedef struct HeavePIDGains {
+  float kp;
+  float ki;
+  float kd;
+} HeavePIDGains;
+
+typedef struct HeavePIDState {
+  float e;
+  float eDiff;
+  float eInt;
+} HeavePIDState;
+
+// ── Heave-only Controller ────────────────────────────────────
+// Single PID on the average gap across all four inductive sensors.
+// Drives all four motor channels with the same PWM magnitude + direction.
+class HeaveController {
+public:
+  bool oor;
+  bool outputOn;
+
+  HeaveController(IndSensorL& f, IndSensorL& b,
+                  HeavePIDGains gains, float avgRef,
+                  bool useFeedforward = true);
+
+  void update();
+  void zeroPWMs();
+  void sendOutputs();
+  void report();
+
+  void updatePID(HeavePIDGains gains);
+  void updateReference(float avgReference);
+
+  // Manual override: drive all channels at -HEAVE_CAP (full attract),
+  // bypassing the PID. OOR still zeroes the output via sendOutputs().
+  void setFullAttract(bool enabled);
+  bool isFullAttract() const { return fullAttract; }
+
+  void setFeedforward(bool enabled) { ffEnabled = enabled; }
+
+private:
+  int16_t pidCompute();
+  int16_t feedforward(float gapMM);
+
+  IndSensorL& Front;
+  IndSensorL& Back;
+
+  HeavePIDGains gains;
+  HeavePIDState state;
+
+  float AvgRef;
+  float avg;
+
+  int16_t PWM;
+  int16_t ffPWM;       // last feedforward value (for debugging/reporting)
+  bool fullAttract;
+  bool ffEnabled;
+};
+
+#endif // HEAVE_CONTROLLER_HPP