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

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