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

90
A0Calibration/generate_lut.py Normal file
View File

@@ -0,0 +1,90 @@
#!/usr/bin/env python3
"""
Generate piecewise-linear LUT arrays for IndSensorLUT.cpp from calibrated xlsx.
Reads A0Calibration/data/Sensor{0,1,2,5}.xlsx (columns: mm_val, avg_adc),
resamples onto a uniform mm grid over the active region, enforces strict
monotonicity, and prints C++ arrays ready to paste into IndSensorLUT.cpp.
Usage: python generate_lut.py [--mm-min 4.0] [--mm-max 16.0] [--step 0.1]
[--sensors 0,1,2,5]
"""
import argparse
from pathlib import Path
import numpy as np
import pandas as pd
DATA_DIR = Path(__file__).parent / 'data'
def build_lut(mm, adc, grid):
order = np.argsort(mm)
mm, adc = mm[order], adc[order]
# Drop duplicate mm (keep mean) so np.interp is well-defined
uniq, inv = np.unique(mm, return_inverse=True)
if len(uniq) != len(mm):
adc = np.array([adc[inv == i].mean() for i in range(len(uniq))])
mm = uniq
lut = np.interp(grid, mm, adc)
lut_i = np.clip(np.round(lut).astype(int), 0, 65535)
# Enforce strict monotonic increasing
for i in range(1, len(lut_i)):
if lut_i[i] <= lut_i[i - 1]:
lut_i[i] = lut_i[i - 1] + 1
return lut_i
def format_array(name, values, per_row=10):
out = [f'static const uint16_t {name}[{len(values)}] = {{']
for i in range(0, len(values), per_row):
row = ', '.join(f'{v:4d}' for v in values[i:i + per_row])
sep = '' if i + per_row >= len(values) else ','
out.append(' ' + row + sep)
out.append('};')
return '\n'.join(out)
def main():
ap = argparse.ArgumentParser()
ap.add_argument('--mm-min', type=float, default=4.0)
ap.add_argument('--mm-max', type=float, default=16.0)
ap.add_argument('--step', type=float, default=0.1)
ap.add_argument('--sensors', type=str, default='0,1,2,5')
args = ap.parse_args()
sensors = [int(s) for s in args.sensors.split(',')]
n = int(round((args.mm_max - args.mm_min) / args.step)) + 1
grid = np.round(args.mm_min + np.arange(n) * args.step, 4)
print(f'// Active range: {args.mm_min}..{args.mm_max} mm @ {args.step} mm '
f'({n} entries per sensor)')
print()
for sn in sensors:
path = DATA_DIR / f'Sensor{sn}.xlsx'
if not path.exists():
print(f'// [skip] {path} not found')
continue
df = pd.read_excel(path)
df = df.dropna(subset=['mm_val', 'avg_adc'])
in_range = (df['mm_val'] >= args.mm_min - args.step) & \
(df['mm_val'] <= args.mm_max + args.step)
if in_range.sum() < 2:
print(f'// [skip] Sensor{sn}: insufficient points in active range')
continue
lut_i = build_lut(df['mm_val'].to_numpy(),
df['avg_adc'].to_numpy(), grid)
print(f'// Sensor{sn} — {len(df)} raw points, '
f'ADC {lut_i[0]}..{lut_i[-1]}')
print(format_array(f'ind{sn}LUT', lut_i))
print()
print('// IndSensorLUT struct initializers:')
for sn in sensors:
print(f'const IndSensorLUT ind{sn}LUTCal = '
f'{{ ind{sn}LUT, {n}, {args.mm_min}f, {args.step}f }};')
if __name__ == '__main__':
main()

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

179
heave_plotter.py Normal file
View File

@@ -0,0 +1,179 @@
#!/usr/bin/env python3
"""
Minimal serial plotter for the HeaveOnly sketch.
Expects CSV lines at 2_000_000 baud: Front,Back,Avg,PWM,outputOn
Key commands (focus the plot window):
0 → output off
1 → output on, PID
2 → output on, full attract
q → quit
Text boxes at the bottom of the window send:
Ref (mm) → R<value>
PID (kp,ki,kd) → P<kp>,<ki>,<kd>
"""
import argparse
import sys
import time
from collections import deque
import matplotlib
matplotlib.use('TkAgg')
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from matplotlib.widgets import TextBox, CheckButtons
import serial
import serial.tools.list_ports
BAUD_RATE = 2_000_000
MAX_POINTS = 400 # rolling window length
TIMEOUT_S = 0.02
def pick_port():
ports = list(serial.tools.list_ports.comports())
if not ports:
sys.exit('No serial ports found.')
for p in ports:
d = (p.description or '').lower()
if 'usbmodem' in p.device or 'arduino' in d or 'usbserial' in p.device.lower():
return p.device
return ports[0].device
def main():
ap = argparse.ArgumentParser()
ap.add_argument('--port', default=None)
ap.add_argument('--window', type=int, default=MAX_POINTS)
args = ap.parse_args()
port = args.port or pick_port()
print(f'Opening {port} @ {BAUD_RATE} baud')
ser = serial.Serial(port, BAUD_RATE, timeout=TIMEOUT_S)
time.sleep(2.0) # let the Arduino reset
ser.reset_input_buffer()
t0 = time.time()
N = args.window
t_buf = deque(maxlen=N)
front = deque(maxlen=N)
back = deque(maxlen=N)
avg = deque(maxlen=N)
pwm = deque(maxlen=N)
on_buf = deque(maxlen=N)
fig, (ax_mm, ax_pwm) = plt.subplots(2, 1, sharex=True, figsize=(10, 6))
fig.subplots_adjust(bottom=0.18)
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)
ax_mm.set_ylabel('Gap (mm)')
ax_mm.grid(True, alpha=0.3)
ax_mm.legend(loc='upper right')
l_pwm, = ax_pwm.plot([], [], label='PWM', color='tab:red')
ax_pwm.axhline(0, color='gray', lw=0.5)
ax_pwm.set_ylabel('PWM')
ax_pwm.set_xlabel('Time (s)')
ax_pwm.set_ylim(-260, 260)
ax_pwm.grid(True, alpha=0.3)
mode_txt = fig.text(0.01, 0.97, 'mode: ?', fontsize=10,
family='monospace', va='top')
fig.suptitle('HeaveOnly — keys: 0=off 1=PID 2=attract q=quit')
def send_mode(cmd_char):
ser.write((cmd_char + '\n').encode())
mode_txt.set_text({'0': 'mode: OFF',
'1': 'mode: PID',
'2': 'mode: ATTRACT'}[cmd_char])
def on_key(event):
# Suppress mode keys while either TextBox is actively editing,
# so typing digits into Ref/PID fields doesn't ping-pong the coils.
if getattr(tb_ref, 'capturekeystrokes', False) or \
getattr(tb_pid, 'capturekeystrokes', False):
return
if event.key in ('0', '1', '2'):
send_mode(event.key)
elif event.key == 'q':
plt.close(fig)
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')
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')
ax_ff = fig.add_axes([0.88, 0.03, 0.08, 0.07])
cb_ff = CheckButtons(ax_ff, ['FF'], [True])
def on_ref(text):
try:
float(text)
except ValueError:
return
ser.write(f'R{text}\n'.encode())
def on_pid(text):
parts = [p.strip() for p in text.split(',')]
if len(parts) != 3:
return
try:
[float(p) for p in parts]
except ValueError:
return
ser.write(f'P{",".join(parts)}\n'.encode())
def on_ff(_label):
ser.write(b'F1\n' if cb_ff.get_status()[0] else b'F0\n')
tb_ref.on_submit(on_ref)
tb_pid.on_submit(on_pid)
cb_ff.on_clicked(on_ff)
def poll_serial():
# Drain everything in the OS buffer each frame
while ser.in_waiting:
raw = ser.readline()
try:
parts = raw.decode('ascii', 'ignore').strip().split(',')
if len(parts) != 5:
continue
f, b, a = float(parts[0]), float(parts[1]), float(parts[2])
p = int(parts[3])
on = int(parts[4])
except ValueError:
continue
t_buf.append(time.time() - t0)
front.append(f); back.append(b); avg.append(a)
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
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_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
ani = FuncAnimation(fig, update, interval=50, blit=False,
cache_frame_data=False)
try:
plt.show()
finally:
try:
ser.write(b'0\n') # safety: turn output off on exit
ser.close()
except Exception:
pass
if __name__ == '__main__':
main()

12
requirements.txt Normal file
View File

@@ -0,0 +1,12 @@
contourpy==1.3.3
cycler==0.12.1
fonttools==4.62.1
kiwisolver==1.5.0
matplotlib==3.10.8
numpy==2.4.4
packaging==26.1
pillow==12.2.0
pyparsing==3.3.2
pyserial==3.5
python-dateutil==2.9.0.post0
six==1.17.0