diff --git a/embedded/HeaveOnly/HeaveOnly.ino b/embedded/HeaveOnly/HeaveOnly.ino index b0b667f..64279e5 100644 --- a/embedded/HeaveOnly/HeaveOnly.ino +++ b/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; diff --git a/embedded/lib/HeaveController.cpp b/embedded/lib/HeaveController.cpp index 74333dc..30dbb89 100644 --- a/embedded/lib/HeaveController.cpp +++ b/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 diff --git a/embedded/lib/HeaveController.hpp b/embedded/lib/HeaveController.hpp index 82a5454..74e042d 100644 --- a/embedded/lib/HeaveController.hpp +++ b/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 diff --git a/heave_plotter.py b/heave_plotter.py index 827fcd5..462bdcc 100644 --- a/heave_plotter.py +++ b/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]) - p = int(parts[3]) - on = int(parts[4]) + f, b, a, r = (float(parts[0]), float(parts[1]), + float(parts[2]), float(parts[3])) + 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)