WOrking with ramp ref

embedded/HeaveOnly/HeaveOnly.ino

@@ -8,7 +8,7 @@
 HeavePIDGains heaveGains = { 400.0f, 0.0f, 300.0f };
 
 // ── Reference ────────────────────────────────────────────────
-float avgRef = 12.2f;  // Target gap height (mm)
+float avgRef = 12.0f;  // Target gap height (mm)
 
 // ── Sampling ─────────────────────────────────────────────────
 #define SAMPLING_RATE 200  // Hz
@@ -16,6 +16,10 @@ float avgRef = 12.2f;  // Target gap height (mm)
 // ── EMA filter alpha (all sensors) ───────────────────────────
 #define ALPHA_VAL 0.7f
 
+// ── Ref Ramp Step (mm per control tick) ──────────────────────
+// At SAMPLING_RATE 200Hz, 0.001f = 0.2 mm/s; 0.005f = 1 mm/s.
+#define RAMP_STEP_MM 0.001f
+
 // ═══════════════════════════════════════════════════════════════
 // ABOVE THIS LINE IS TUNING VALUES ONLY, BELOW IS ACTUAL CODE.
 // ═══════════════════════════════════════════════════════════════
@@ -23,7 +27,7 @@ float avgRef = 12.2f;  // Target gap height (mm)
 unsigned long tprior;
 unsigned int tDiffMicros;
 
-HeaveController controller(indF, indB, heaveGains, avgRef);
+HeaveController controller(indF, indB, heaveGains, avgRef, RAMP_STEP_MM);
 
 const int dt_micros = 1000000 / SAMPLING_RATE;
 

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),
+    AvgRef(avgRef), targetRef(avgRef), rampStep(rampStep_),
-    fullAttract(false), ffEnabled(useFeedforward)
+    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

heave_plotter.py

@@ -2,7 +2,7 @@
 """
 Minimal serial plotter for the HeaveOnly sketch.
 
-Expects CSV lines at 2_000_000 baud:  Front,Back,Avg,PWM,outputOn
+Expects CSV lines at 2_000_000 baud:  Front,Back,Avg,ActiveRef,PWM,outputOn
 Key commands (focus the plot window):
   0 → output off
   1 → output on, PID
@@ -61,6 +61,7 @@ def main():
     front    = deque(maxlen=N)
     back     = deque(maxlen=N)
     avg      = deque(maxlen=N)
+    aref     = deque(maxlen=N)
     pwm      = deque(maxlen=N)
     on_buf   = deque(maxlen=N)
 
@@ -69,6 +70,8 @@ def main():
     l_front, = ax_mm.plot([], [], label='Front', color='tab:blue')
     l_back,  = ax_mm.plot([], [], label='Back',  color='tab:orange')
     l_avg,   = ax_mm.plot([], [], label='Avg',   color='k', lw=2)
+    l_aref,  = ax_mm.plot([], [], label='ActiveRef', color='tab:green',
+                          lw=1.2, ls='--')
     ax_mm.set_ylabel('Gap (mm)')
     ax_mm.grid(True, alpha=0.3)
     ax_mm.legend(loc='upper right')
@@ -103,11 +106,11 @@ def main():
     fig.canvas.mpl_connect('key_press_event', on_key)
 
     ax_ref = fig.add_axes([0.10, 0.04, 0.15, 0.05])
-    tb_ref = TextBox(ax_ref, 'Ref (mm) ', initial='12.36')
+    tb_ref = TextBox(ax_ref, 'Ref (mm) ', initial='12')
     ax_pid = fig.add_axes([0.50, 0.04, 0.30, 0.05])
-    tb_pid = TextBox(ax_pid, 'PID (kp,ki,kd) ', initial='10,0,8')
+    tb_pid = TextBox(ax_pid, 'PID (kp,ki,kd) ', initial='400,0,300')
     ax_ff = fig.add_axes([0.88, 0.03, 0.08, 0.07])
-    cb_ff = CheckButtons(ax_ff, ['FF'], [True])
+    cb_ff = CheckButtons(ax_ff, ['FF'], [False])
 
     def on_ref(text):
         try:
@@ -139,29 +142,31 @@ def main():
             raw = ser.readline()
             try:
                 parts = raw.decode('ascii', 'ignore').strip().split(',')
-                if len(parts) != 5:
+                if len(parts) != 6:
                     continue
-                f, b, a = float(parts[0]), float(parts[1]), float(parts[2])
+                f, b, a, r = (float(parts[0]), float(parts[1]),
-                p = int(parts[3])
+                              float(parts[2]), float(parts[3]))
-                on = int(parts[4])
+                p = int(parts[4])
+                on = int(parts[5])
             except ValueError:
                 continue
             t_buf.append(time.time() - t0)
-            front.append(f); back.append(b); avg.append(a)
+            front.append(f); back.append(b); avg.append(a); aref.append(r)
             pwm.append(p);   on_buf.append(on)
 
     def update(_frame):
         poll_serial()
         if not t_buf:
-            return l_front, l_back, l_avg, l_pwm, mode_txt
+            return l_front, l_back, l_avg, l_aref, l_pwm, mode_txt
         xs = list(t_buf)
         l_front.set_data(xs, list(front))
         l_back .set_data(xs, list(back))
         l_avg  .set_data(xs, list(avg))
+        l_aref .set_data(xs, list(aref))
         l_pwm  .set_data(xs, list(pwm))
         ax_mm.relim();  ax_mm.autoscale_view(scalex=True, scaley=True)
         ax_pwm.set_xlim(xs[0], max(xs[-1], xs[0] + 1e-3))
-        return l_front, l_back, l_avg, l_pwm, mode_txt
+        return l_front, l_back, l_avg, l_aref, l_pwm, mode_txt
 
     ani = FuncAnimation(fig, update, interval=50, blit=False,
                         cache_frame_data=False)