heave control + plotter
This commit is contained in:
125
embedded/lib/HeaveController.cpp
Normal file
125
embedded/lib/HeaveController.cpp
Normal 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
87
embedded/lib/HeaveController.hpp
Normal 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
149
embedded/lib/IndSensorLUT.cpp
Normal 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
47
embedded/lib/IndSensorLUT.hpp
Normal 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
|
||||
Reference in New Issue
Block a user