WOrking with ramp ref

embedded/lib/HeaveController.cpp

@@ -19,12 +19,14 @@ static const int16_t HEAVE_FF_LUT[HEAVE_FF_LUT_SIZE] PROGMEM = {
 
 HeaveController::HeaveController(
     IndSensorL& f, IndSensorL& b,
-    HeavePIDGains g, float avgRef, bool useFeedforward)
+    HeavePIDGains g, float avgRef, float rampStep_, bool useFeedforward)
   : oor(false), outputOn(false),
     Front(f), Back(b),
     gains(g), state({0, 0, 0}),
-    AvgRef(avgRef), avg(0), PWM(0), ffPWM(0),
-    fullAttract(false), ffEnabled(useFeedforward)
+    AvgRef(avgRef), targetRef(avgRef), rampStep(rampStep_),
+    avg(0), PWM(0), ffPWM(0),
+    fullAttract(false), ffEnabled(useFeedforward),
+    prevPidActive(false)
 {}
 
 void HeaveController::update() {
@@ -35,6 +37,22 @@ void HeaveController::update() {
 
   avg = (Front.mmVal + Back.mmVal) * 0.5f;
 
+  // Seed AvgRef from current position on entry to PID-active mode, then
+  // step it toward targetRef by rampStep each tick. This applies to both
+  // off→on (cmd '1') and full-attract→PID (cmd '2'→'1') transitions.
+  bool pidActive = outputOn && !fullAttract;
+  if (pidActive) {
+    if (!prevPidActive) {
+      AvgRef = avg;
+      state.eInt = 0;
+    }
+    float delta = targetRef - AvgRef;
+    if (delta >  rampStep)       AvgRef += rampStep;
+    else if (delta < -rampStep)  AvgRef -= rampStep;
+    else                         AvgRef = targetRef;
+  }
+  prevPidActive = pidActive;
+
   float e = AvgRef - avg;
   state.eDiff = e - state.e;
   if (!oor && !fullAttract) {
@@ -109,16 +127,17 @@ void HeaveController::sendOutputs() {
 }
 
 void HeaveController::report() {
-  // CSV: Front,Back,Avg,PWM,ControlOn
+  // CSV: Front,Back,Avg,ActiveRef,PWM,ControlOn
   Serial.print(Front.mmVal); Serial.print(',');
   Serial.print(Back.mmVal);  Serial.print(',');
   Serial.print(avg);         Serial.print(',');
+  Serial.print(AvgRef);      Serial.print(',');
   Serial.print(PWM);         Serial.print(',');
   Serial.println(outputOn);
 }
 
 void HeaveController::updatePID(HeavePIDGains g) { gains = g; }
-void HeaveController::updateReference(float avgReference) { AvgRef = avgReference; }
+void HeaveController::updateReference(float avgReference) { targetRef = avgReference; }
 void HeaveController::setFullAttract(bool enabled) {
   fullAttract = enabled;
   if (enabled) state.eInt = 0;  // drop stale integral so PID resume is clean

embedded/lib/HeaveController.hpp

@@ -25,6 +25,11 @@
 #define HEAVE_FF_GAP_MAX  20.0f
 #define HEAVE_FF_GAP_STEP 0.269841f
 
+// ── Reference Ramp ───────────────────────────────────────────
+// Per-update step the active ref moves toward the target. At 200Hz tick
+// rate, 0.001f mm/tick = 0.2 mm/s. Override via constructor.
+#define HEAVE_DEFAULT_RAMP_STEP 0.001f
+
 // ── PID Gains / State ────────────────────────────────────────
 typedef struct HeavePIDGains {
   float kp;
@@ -48,7 +53,8 @@ public:
 
   HeaveController(IndSensorL& f, IndSensorL& b,
                   HeavePIDGains gains, float avgRef,
-                  bool useFeedforward = true);
+                  float rampStep = HEAVE_DEFAULT_RAMP_STEP,
+                  bool useFeedforward = false);
 
   void update();
   void zeroPWMs();
@@ -75,13 +81,16 @@ private:
   HeavePIDGains gains;
   HeavePIDState state;
 
-  float AvgRef;
+  float AvgRef;        // active ref the PID tracks; ramps toward targetRef
+  float targetRef;     // final desired ref (set by updateReference)
+  float rampStep;      // per-update step size for AvgRef → targetRef
   float avg;
 
   int16_t PWM;
   int16_t ffPWM;       // last feedforward value (for debugging/reporting)
   bool fullAttract;
   bool ffEnabled;
+  bool prevPidActive;  // transition detector for seeding AvgRef = avg
 };
 
 #endif // HEAVE_CONTROLLER_HPP