adipu/guadaloop_lev_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

WOrking with ramp ref

Browse Source
This commit is contained in:
pulipakaa24
2026-04-17 10:37:13 -05:00
parent cef8106fd6
commit 58bc1a43d8
4 changed files with 57 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
embedded/HeaveOnly/HeaveOnly.ino
View File

@@ -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;

29
embedded/lib/HeaveController.cpp
View File

@@ -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

13
embedded/lib/HeaveController.hpp
View File

@@ -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

27
heave_plotter.py
View File

@@ -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)