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

heave control + plotter

Browse Source
This commit is contained in:
pulipakaa24
2026-04-16 15:22:54 -05:00
parent 7c54fe38e3
commit cef8106fd6
18 changed files with 817 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
embedded/AdditiveControlCode/IndSensorLUT.cpp Symbolic link
View File

@@ -0,0 +1 @@
../lib/IndSensorLUT.cpp

1
embedded/AdditiveControlCode/IndSensorLUT.hpp Symbolic link
View File

@@ -0,0 +1 @@
../lib/IndSensorLUT.hpp

1
embedded/HeaveOnly/ADC.cpp Symbolic link
View File

@@ -0,0 +1 @@
../lib/ADC.cpp

1
embedded/HeaveOnly/ADC.hpp Symbolic link
View File

@@ -0,0 +1 @@
../lib/ADC.hpp

1
embedded/HeaveOnly/FastPWM.cpp Symbolic link
View File

@@ -0,0 +1 @@
../lib/FastPWM.cpp

1
embedded/HeaveOnly/FastPWM.hpp Symbolic link
View File

@@ -0,0 +1 @@
../lib/FastPWM.hpp

1
embedded/HeaveOnly/HeaveController.cpp Symbolic link
View File

@@ -0,0 +1 @@
../lib/HeaveController.cpp

1
embedded/HeaveOnly/HeaveController.hpp Symbolic link
View File

@@ -0,0 +1 @@
../lib/HeaveController.hpp

118
embedded/HeaveOnly/HeaveOnly.ino Normal file
View File

@@ -0,0 +1,118 @@
#include <Arduino.h>
#include "IndSensorLUT.hpp"
#include "HeaveController.hpp"
#include "ADC.hpp"
#include "FastPWM.hpp"
// ── PID Gains (Kp, Ki, Kd) ──────────────────────────────────
HeavePIDGains heaveGains = { 400.0f, 0.0f, 300.0f };
// ── Reference ────────────────────────────────────────────────
float avgRef = 12.2f; // Target gap height (mm)
// ── Sampling ─────────────────────────────────────────────────
#define SAMPLING_RATE 200 // Hz
// ── EMA filter alpha (all sensors) ───────────────────────────
#define ALPHA_VAL 0.7f
// ═══════════════════════════════════════════════════════════════
// ABOVE THIS LINE IS TUNING VALUES ONLY, BELOW IS ACTUAL CODE.
// ═══════════════════════════════════════════════════════════════
unsigned long tprior;
unsigned int tDiffMicros;
HeaveController controller(indF, indB, heaveGains, avgRef);
const int dt_micros = 1000000 / SAMPLING_RATE;
void setup() {
Serial.begin(2000000);
setupADC();
setupFastPWM();
indF.alpha = ALPHA_VAL;
indB.alpha = ALPHA_VAL;
tprior = micros();
pinMode(dirFL, OUTPUT); pinMode(pwmFL, OUTPUT);
pinMode(dirBL, OUTPUT); pinMode(pwmBL, OUTPUT);
pinMode(dirFR, OUTPUT); pinMode(pwmFR, OUTPUT);
pinMode(dirBR, OUTPUT); pinMode(pwmBR, OUTPUT);
}
// Dispatch a newline-terminated command line.
void handleCommand(char* line) {
switch (line[0]) {
case '0':
controller.outputOn = false;
controller.setFullAttract(false);
break;
case '1':
controller.outputOn = true;
controller.setFullAttract(false);
break;
case '2':
controller.outputOn = true;
controller.setFullAttract(true);
break;
case 'R': {
avgRef = atof(line + 1);
controller.updateReference(avgRef);
break;
}
case 'P': {
char* kpTok = strtok(line + 1, ",");
char* kiTok = strtok(NULL, ",");
char* kdTok = strtok(NULL, ",");
if (kpTok && kiTok && kdTok) {
heaveGains = { (float)atof(kpTok),
(float)atof(kiTok),
(float)atof(kdTok) };
controller.updatePID(heaveGains);
}
break;
}
case 'F':
// F1 / F0 — toggle feedforward
controller.setFeedforward(line[1] != '0');
break;
default:
break;
}
}
void loop() {
// Serial commands (all newline-terminated):
// 0 → output off
// 1 → output on, PID control
// 2 → output on, full attract
// R<float> → update reference (mm) e.g. R12.5
// P<kp>,<ki>,<kd> → update PID gains e.g. P10,0,8
// F1 / F0 → feedforward on/off
static char lineBuf[40];
static uint8_t lineLen = 0;
while (Serial.available() > 0) {
char c = Serial.read();
if (c == '\n' || c == '\r') {
if (lineLen == 0) continue;
lineBuf[lineLen] = '\0';
handleCommand(lineBuf);
lineLen = 0;
} else if (lineLen < sizeof(lineBuf) - 1) {
lineBuf[lineLen++] = c;
}
}
tDiffMicros = micros() - tprior;
if (tDiffMicros >= dt_micros) {
controller.update();
controller.report();
controller.sendOutputs();
tprior = micros();
}
}

1
embedded/HeaveOnly/IndSensorLUT.cpp Symbolic link
View File

@@ -0,0 +1 @@
../lib/IndSensorLUT.cpp

1
embedded/HeaveOnly/IndSensorLUT.hpp Symbolic link
View File

@@ -0,0 +1 @@
../lib/IndSensorLUT.hpp

125
embedded/lib/HeaveController.cpp Normal file
View File

@@ -0,0 +1,125 @@
#include "HeaveController.hpp"
#include <Arduino.h>
#include <avr/pgmspace.h>
static const float MAX_INTEGRAL = 1e4f;
// Gap [mm] → equilibrium PWM (+ = repel, - = attract).
// 64 entries over 3–20 mm at 0.269841 mm steps. Copied from Controller.cpp.
static const int16_t HEAVE_FF_LUT[HEAVE_FF_LUT_SIZE] PROGMEM = {
238, 238, 238, 238, 238, 238, 238, 238,
238, 238, 238, 238, 238, 238, 238, 238,
238, 238, 234, 219, 204, 188, 172, 157,
141, 125, 109, 93, 77, 61, 45, 29,
13, -3, -19, -35, -51, -67, -84, -100,
-116, -133, -150, -166, -183, -200, -217, -234,
-250, -250, -250, -250, -250, -250, -250, -250,
-250, -250, -250, -250, -250, -250, -250, -250
};
HeaveController::HeaveController(
IndSensorL& f, IndSensorL& b,
HeavePIDGains g, float avgRef, 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)
{}
void HeaveController::update() {
Front.readMM();
Back.readMM();
oor = Front.oor || Back.oor;
avg = (Front.mmVal + Back.mmVal) * 0.5f;
float e = AvgRef - avg;
state.eDiff = e - state.e;
if (!oor && !fullAttract) {
state.eInt += e;
state.eInt = constrain(state.eInt, -MAX_INTEGRAL, MAX_INTEGRAL);
}
state.e = e;
ffPWM = ffEnabled ? feedforward(avg) : 0;
if (fullAttract) {
PWM = -HEAVE_CAP; // manual override — ignore OOR, drive coils unconditionally
} else if (oor) {
// Sensor out of LUT range → trust feedforward alone (PID input is clamped).
// avg is clamped to [mmMin, mmMax] so FF saturates toward repel when too close,
// attract when too far — which is exactly the bring-up behavior.
PWM = ffPWM;
} else {
// Gain-schedule PID output by (z/z_ref)² to linearize 1/z² force law.
// Floor z to 2mm so sensor dropouts don't collapse control authority.
// float z = max(avg, 2.0f);
// float scale = (z * z) / (AvgRef * AvgRef);
// PWM = constrain(ffPWM + (int16_t)(scale * pidCompute()), -HEAVE_CAP, HEAVE_CAP); // In range: feedforward provides gravity/equilibrium bias, PID corrects residual.
PWM = constrain(ffPWM + pidCompute(), -HEAVE_CAP, HEAVE_CAP);
}
}
// Linearly interpolate the PROGMEM feedforward LUT.
int16_t HeaveController::feedforward(float gapMM) {
if (gapMM <= HEAVE_FF_GAP_MIN) return (int16_t)pgm_read_word(&HEAVE_FF_LUT[0]);
if (gapMM >= HEAVE_FF_GAP_MAX) return (int16_t)pgm_read_word(&HEAVE_FF_LUT[HEAVE_FF_LUT_SIZE - 1]);
float idx_f = (gapMM - HEAVE_FF_GAP_MIN) / HEAVE_FF_GAP_STEP;
uint8_t idx = (uint8_t)idx_f;
if (idx >= HEAVE_FF_LUT_SIZE - 1) idx = HEAVE_FF_LUT_SIZE - 2;
int16_t v0 = (int16_t)pgm_read_word(&HEAVE_FF_LUT[idx]);
int16_t v1 = (int16_t)pgm_read_word(&HEAVE_FF_LUT[idx + 1]);
float frac = idx_f - (float)idx;
return (int16_t)(v0 + frac * (v1 - v0));
}
int16_t HeaveController::pidCompute() {
if (oor) return 0;
float out = gains.kp * state.e
+ gains.ki * state.eInt
+ gains.kd * state.eDiff;
return (int16_t)constrain(out, -HEAVE_CAP, HEAVE_CAP);
}
void HeaveController::zeroPWMs() {
PWM = 0;
}
// Drive all four motor channels identically. Direction bit mirrors the sign
// convention from Controller.cpp: LOW = repelling (PWM > 0), HIGH = attracting.
void HeaveController::sendOutputs() {
if (!outputOn) zeroPWMs();
bool attract = (PWM < 0);
uint8_t mag = (uint8_t)abs(PWM);
digitalWrite(dirFL, attract);
digitalWrite(dirBL, attract);
digitalWrite(dirFR, attract);
digitalWrite(dirBR, attract);
OCR2A = mag; // Pin 11 (FL)
OCR1A = mag; // Pin 9 (FR)
OCR2B = mag; // Pin 3 (BL)
OCR1B = mag; // Pin 10 (BR)
}
void HeaveController::report() {
// CSV: Front,Back,Avg,PWM,ControlOn
Serial.print(Front.mmVal); Serial.print(',');
Serial.print(Back.mmVal); Serial.print(',');
Serial.print(avg); 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::setFullAttract(bool enabled) {
fullAttract = enabled;
if (enabled) state.eInt = 0; // drop stale integral so PID resume is clean
}

87
embedded/lib/HeaveController.hpp Normal file
View File

@@ -0,0 +1,87 @@
#ifndef HEAVE_CONTROLLER_HPP
#define HEAVE_CONTROLLER_HPP
#include <stdint.h>
#include "IndSensorLUT.hpp"
// ── Pin Mapping (mirrors Controller.hpp) ─────────────────────
#define dirBL 2
#define pwmBL 3
#define dirBR 4
#define pwmBR 10
#define pwmFL 11
#define dirFL 7
#define dirFR 8
#define pwmFR 9
// ── Output Cap ───────────────────────────────────────────────
#define HEAVE_CAP 250
// ── Feedforward LUT (gap mm → equilibrium PWM) ───────────────
// Source: FF_PWM_LUT in Controller.cpp (pod 9.4 kg, R 1.1 Ω, V 12 V).
// Positive = repel, negative = attract. Lives in PROGMEM.
#define HEAVE_FF_LUT_SIZE 64
#define HEAVE_FF_GAP_MIN 3.0f
#define HEAVE_FF_GAP_MAX 20.0f
#define HEAVE_FF_GAP_STEP 0.269841f
// ── PID Gains / State ────────────────────────────────────────
typedef struct HeavePIDGains {
float kp;
float ki;
float kd;
} HeavePIDGains;
typedef struct HeavePIDState {
float e;
float eDiff;
float eInt;
} HeavePIDState;
// ── Heave-only Controller ────────────────────────────────────
// Single PID on the average gap across all four inductive sensors.
// Drives all four motor channels with the same PWM magnitude + direction.
class HeaveController {
public:
bool oor;
bool outputOn;
HeaveController(IndSensorL& f, IndSensorL& b,
HeavePIDGains gains, float avgRef,
bool useFeedforward = true);
void update();
void zeroPWMs();
void sendOutputs();
void report();
void updatePID(HeavePIDGains gains);
void updateReference(float avgReference);
// Manual override: drive all channels at -HEAVE_CAP (full attract),
// bypassing the PID. OOR still zeroes the output via sendOutputs().
void setFullAttract(bool enabled);
bool isFullAttract() const { return fullAttract; }
void setFeedforward(bool enabled) { ffEnabled = enabled; }
private:
int16_t pidCompute();
int16_t feedforward(float gapMM);
IndSensorL& Front;
IndSensorL& Back;
HeavePIDGains gains;
HeavePIDState state;
float AvgRef;
float avg;
int16_t PWM;
int16_t ffPWM; // last feedforward value (for debugging/reporting)
bool fullAttract;
bool ffEnabled;
};
#endif // HEAVE_CONTROLLER_HPP

149
embedded/lib/IndSensorLUT.cpp Normal file
View File

@@ -0,0 +1,149 @@
#include "IndSensorLUT.hpp"
#include <Arduino.h>
#include <avr/pgmspace.h>
#include "ADC.hpp"
// LUT active range: 4.0..18.0 mm at 0.1 mm resolution (141 entries each).
// Generated from A0Calibration/data/Sensor{0,1,2,5}.xlsx via linear interpolation
// onto a uniform mm grid; strictly monotonic increasing in ADC.
static const uint16_t ind0LUT[141] PROGMEM = {
67, 71, 74, 77, 81, 85, 99, 102, 108, 113,
117, 120, 125, 135, 141, 151, 154, 158, 165, 170,
181, 194, 196, 207, 212, 218, 223, 233, 234, 250,
265, 267, 279, 284, 291, 296, 301, 309, 327, 334,
346, 350, 357, 360, 366, 375, 379, 387, 411, 421,
422, 429, 434, 441, 451, 462, 470, 480, 487, 488,
497, 502, 505, 515, 523, 530, 537, 545, 546, 559,
566, 570, 575, 587, 592, 596, 601, 612, 618, 625,
628, 632, 636, 640, 646, 653, 658, 663, 668, 672,
678, 684, 686, 688, 691, 699, 703, 708, 709, 711,
715, 719, 722, 726, 729, 733, 737, 739, 743, 747,
748, 751, 754, 759, 760, 763, 766, 769, 770, 772,
775, 776, 779, 781, 783, 785, 788, 791, 792, 794,
796, 797, 798, 800, 802, 803, 806, 807, 808, 810,
811
};
static const uint16_t ind1LUT[141] PROGMEM = {
64, 68, 72, 77, 80, 87, 93, 96, 100, 109,
114, 121, 127, 133, 139, 146, 152, 164, 169, 176,
183, 192, 200, 207, 217, 224, 234, 242, 251, 259,
270, 280, 289, 299, 310, 319, 329, 337, 348, 358,
363, 377, 386, 395, 407, 416, 428, 437, 445, 453,
462, 475, 485, 497, 505, 515, 527, 532, 547, 553,
564, 573, 582, 591, 603, 611, 619, 626, 634, 644,
653, 658, 667, 675, 683, 691, 700, 704, 713, 721,
728, 735, 742, 748, 755, 762, 768, 774, 781, 786,
793, 798, 804, 809, 815, 820, 826, 830, 835, 840,
844, 849, 854, 859, 863, 867, 870, 874, 878, 881,
887, 890, 894, 896, 899, 902, 905, 909, 911, 914,
916, 921, 923, 926, 929, 932, 934, 937, 940, 942,
944, 946, 949, 951, 953, 955, 957, 959, 961, 962,
965
};
static const uint16_t ind2LUT[141] PROGMEM = {
58, 60, 65, 73, 76, 79, 85, 90, 94, 97,
101, 105, 114, 121, 126, 131, 136, 142, 148, 153,
163, 172, 178, 183, 188, 194, 202, 216, 226, 231,
241, 246, 253, 263, 271, 276, 284, 293, 306, 313,
318, 326, 337, 345, 353, 361, 369, 387, 394, 400,
406, 413, 420, 426, 434, 443, 450, 462, 467, 468,
475, 481, 486, 494, 501, 510, 518, 525, 531, 536,
542, 546, 556, 561, 567, 572, 577, 582, 588, 594,
599, 606, 612, 615, 623, 627, 631, 634, 638, 641,
646, 654, 657, 660, 664, 668, 672, 675, 679, 684,
687, 691, 694, 698, 701, 704, 706, 708, 711, 714,
716, 719, 722, 725, 727, 728, 732, 735, 737, 739,
740, 743, 745, 746, 748, 750, 751, 752, 755, 758,
760, 761, 762, 764, 765, 768, 769, 770, 771, 772,
773
};
static const uint16_t ind5LUT[141] PROGMEM = {
44, 47, 50, 54, 59, 62, 64, 69, 72, 77,
81, 86, 90, 97, 101, 104, 113, 118, 122, 127,
132, 139, 147, 154, 159, 167, 175, 180, 188, 193,
201, 212, 222, 228, 234, 239, 253, 258, 265, 273,
285, 292, 300, 307, 315, 323, 335, 349, 355, 364,
371, 375, 384, 392, 399, 407, 418, 424, 433, 440,
448, 455, 464, 472, 482, 488, 494, 501, 509, 520,
529, 536, 541, 548, 554, 558, 564, 574, 579, 586,
591, 598, 607, 611, 617, 624, 627, 632, 638, 646,
652, 657, 661, 665, 670, 675, 682, 687, 691, 695,
698, 703, 709, 712, 716, 719, 722, 728, 731, 735,
739, 743, 747, 749, 752, 754, 758, 761, 764, 768,
770, 773, 775, 777, 781, 784, 787, 789, 791, 792,
795, 797, 799, 801, 803, 805, 807, 808, 810, 813,
814
};
const IndSensorLUT ind0LUTCal = { ind0LUT, 141, 4.0f, 0.1f };
const IndSensorLUT ind1LUTCal = { ind1LUT, 141, 4.0f, 0.1f };
const IndSensorLUT ind2LUTCal = { ind2LUT, 141, 4.0f, 0.1f };
const IndSensorLUT ind5LUTCal = { ind5LUT, 141, 4.0f, 0.1f };
IndSensorL::IndSensorL(IndSensorLUT calibration, uint8_t analogPin, float emaAlpha)
: oor(false), oorDir(0), mmVal(0.0f), analog(0), alpha(emaAlpha),
cal(calibration), pin(analogPin), filteredRaw(0.0f) {
filteredRaw = analogRead(pin);
}
// Binary search the monotonic ADC table for `raw`, then linearly interpolate mm.
// LUT lives in Flash (PROGMEM); all reads go through pgm_read_word().
// Sets oor + oorDir when raw falls outside the calibrated range.
float IndSensorL::toMM(uint16_t raw) {
const uint16_t* t = cal.adcTable;
const uint16_t n = cal.length;
uint16_t t0 = pgm_read_word(&t[0]);
uint16_t tEnd = pgm_read_word(&t[n - 1]);
if (raw <= t0) {
oor = true;
oorDir = -1; // ADC below table → gap below mmMin → too close
return cal.mmMin;
}
if (raw >= tEnd) {
oor = true;
oorDir = +1; // ADC above table → gap above mmMax → too far
return cal.mmMin + (n - 1) * cal.mmStep;
}
uint16_t lo = 0, hi = n - 1;
while (hi - lo > 1) {
uint16_t mid = (lo + hi) >> 1;
if (pgm_read_word(&t[mid]) <= raw) lo = mid;
else hi = mid;
}
uint16_t vLo = pgm_read_word(&t[lo]);
uint16_t vHi = pgm_read_word(&t[hi]);
float frac = (float)(raw - vLo) / (float)(vHi - vLo);
return cal.mmMin + ((float)lo + frac) * cal.mmStep;
}
float IndSensorL::readMM() {
uint8_t index = pin - A0;
index = (index > 3) ? index - 2 : index;
uint16_t raw;
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
raw = adc_results[index];
}
filteredRaw = alpha * raw + (1.0f - alpha) * filteredRaw;
analog = (uint16_t)filteredRaw;
oor = false;
oorDir = 0;
mmVal = toMM(analog);
return mmVal;
}
// Face → sensor pin/LUT mapping (assigned by user).
IndSensorL indF(ind2LUTCal, A5);
IndSensorL indL(ind1LUTCal, A1);
IndSensorL indR(ind0LUTCal, A0);
IndSensorL indB(ind5LUTCal, A4);

47
embedded/lib/IndSensorLUT.hpp Normal file
View File

@@ -0,0 +1,47 @@
#ifndef IND_SENSOR_LUT_HPP
#define IND_SENSOR_LUT_HPP
#include <stdint.h>
// Piecewise-linear LUT calibration for an inductive sensor.
// adcTable is ADC value at mm = mmMin + i*mmStep, monotonically increasing in i.
typedef struct IndSensorLUT {
const uint16_t* adcTable;
uint16_t length;
float mmMin;
float mmStep;
} IndSensorLUT;
class IndSensorL {
public:
bool oor;
int8_t oorDir; // -1 = too close (below LUT), +1 = too far (above LUT), 0 = in range
float mmVal;
uint16_t analog;
float alpha;
IndSensorL(IndSensorLUT calibration, uint8_t analogPin, float emaAlpha = 0.3f);
float readMM();
private:
IndSensorLUT cal;
uint8_t pin;
float filteredRaw;
float toMM(uint16_t raw);
};
// Per-sensor LUTs generated from A0Calibration/data (4.0..18.0 mm @ 0.1 mm).
extern const IndSensorLUT ind0LUTCal;
extern const IndSensorLUT ind1LUTCal;
extern const IndSensorLUT ind2LUTCal;
extern const IndSensorLUT ind5LUTCal;
// Sensor instances — face → pin/LUT mapping defined in IndSensorLUT.cpp.
// Names shadow IndSensorMap; do not include both headers in the same sketch.
extern IndSensorL indF;
extern IndSensorL indB;
extern IndSensorL indL;
extern IndSensorL indR;
#endif // IND_SENSOR_LUT_HPP