From cef8106fd6c4a8db2a80f1f857bbd6bd70401b7c Mon Sep 17 00:00:00 2001 From: pulipakaa24 Date: Thu, 16 Apr 2026 15:22:54 -0500 Subject: [PATCH] heave control + plotter --- A0Calibration/generate_lut.py | 90 +++++++++ embedded/AdditiveControlCode/IndSensorLUT.cpp | 1 + embedded/AdditiveControlCode/IndSensorLUT.hpp | 1 + embedded/HeaveOnly/ADC.cpp | 1 + embedded/HeaveOnly/ADC.hpp | 1 + embedded/HeaveOnly/FastPWM.cpp | 1 + embedded/HeaveOnly/FastPWM.hpp | 1 + embedded/HeaveOnly/HeaveController.cpp | 1 + embedded/HeaveOnly/HeaveController.hpp | 1 + embedded/HeaveOnly/HeaveOnly.ino | 118 ++++++++++++ embedded/HeaveOnly/IndSensorLUT.cpp | 1 + embedded/HeaveOnly/IndSensorLUT.hpp | 1 + embedded/lib/HeaveController.cpp | 125 ++++++++++++ embedded/lib/HeaveController.hpp | 87 +++++++++ embedded/lib/IndSensorLUT.cpp | 149 +++++++++++++++ embedded/lib/IndSensorLUT.hpp | 47 +++++ heave_plotter.py | 179 ++++++++++++++++++ requirements.txt | 12 ++ 18 files changed, 817 insertions(+) create mode 100644 A0Calibration/generate_lut.py create mode 120000 embedded/AdditiveControlCode/IndSensorLUT.cpp create mode 120000 embedded/AdditiveControlCode/IndSensorLUT.hpp create mode 120000 embedded/HeaveOnly/ADC.cpp create mode 120000 embedded/HeaveOnly/ADC.hpp create mode 120000 embedded/HeaveOnly/FastPWM.cpp create mode 120000 embedded/HeaveOnly/FastPWM.hpp create mode 120000 embedded/HeaveOnly/HeaveController.cpp create mode 120000 embedded/HeaveOnly/HeaveController.hpp create mode 100644 embedded/HeaveOnly/HeaveOnly.ino create mode 120000 embedded/HeaveOnly/IndSensorLUT.cpp create mode 120000 embedded/HeaveOnly/IndSensorLUT.hpp create mode 100644 embedded/lib/HeaveController.cpp create mode 100644 embedded/lib/HeaveController.hpp create mode 100644 embedded/lib/IndSensorLUT.cpp create mode 100644 embedded/lib/IndSensorLUT.hpp create mode 100644 heave_plotter.py create mode 100644 requirements.txt diff --git a/A0Calibration/generate_lut.py b/A0Calibration/generate_lut.py new file mode 100644 index 0000000..bff5eba --- /dev/null +++ b/A0Calibration/generate_lut.py @@ -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() diff --git a/embedded/AdditiveControlCode/IndSensorLUT.cpp b/embedded/AdditiveControlCode/IndSensorLUT.cpp new file mode 120000 index 0000000..960360c --- /dev/null +++ b/embedded/AdditiveControlCode/IndSensorLUT.cpp @@ -0,0 +1 @@ +../lib/IndSensorLUT.cpp \ No newline at end of file diff --git a/embedded/AdditiveControlCode/IndSensorLUT.hpp b/embedded/AdditiveControlCode/IndSensorLUT.hpp new file mode 120000 index 0000000..e82eaa0 --- /dev/null +++ b/embedded/AdditiveControlCode/IndSensorLUT.hpp @@ -0,0 +1 @@ +../lib/IndSensorLUT.hpp \ No newline at end of file diff --git a/embedded/HeaveOnly/ADC.cpp b/embedded/HeaveOnly/ADC.cpp new file mode 120000 index 0000000..a2bcc20 --- /dev/null +++ b/embedded/HeaveOnly/ADC.cpp @@ -0,0 +1 @@ +../lib/ADC.cpp \ No newline at end of file diff --git a/embedded/HeaveOnly/ADC.hpp b/embedded/HeaveOnly/ADC.hpp new file mode 120000 index 0000000..05bcbaf --- /dev/null +++ b/embedded/HeaveOnly/ADC.hpp @@ -0,0 +1 @@ +../lib/ADC.hpp \ No newline at end of file diff --git a/embedded/HeaveOnly/FastPWM.cpp b/embedded/HeaveOnly/FastPWM.cpp new file mode 120000 index 0000000..b4f3868 --- /dev/null +++ b/embedded/HeaveOnly/FastPWM.cpp @@ -0,0 +1 @@ +../lib/FastPWM.cpp \ No newline at end of file diff --git a/embedded/HeaveOnly/FastPWM.hpp b/embedded/HeaveOnly/FastPWM.hpp new file mode 120000 index 0000000..06aee82 --- /dev/null +++ b/embedded/HeaveOnly/FastPWM.hpp @@ -0,0 +1 @@ +../lib/FastPWM.hpp \ No newline at end of file diff --git a/embedded/HeaveOnly/HeaveController.cpp b/embedded/HeaveOnly/HeaveController.cpp new file mode 120000 index 0000000..75a4247 --- /dev/null +++ b/embedded/HeaveOnly/HeaveController.cpp @@ -0,0 +1 @@ +../lib/HeaveController.cpp \ No newline at end of file diff --git a/embedded/HeaveOnly/HeaveController.hpp b/embedded/HeaveOnly/HeaveController.hpp new file mode 120000 index 0000000..4e29832 --- /dev/null +++ b/embedded/HeaveOnly/HeaveController.hpp @@ -0,0 +1 @@ +../lib/HeaveController.hpp \ No newline at end of file diff --git a/embedded/HeaveOnly/HeaveOnly.ino b/embedded/HeaveOnly/HeaveOnly.ino new file mode 100644 index 0000000..b0b667f --- /dev/null +++ b/embedded/HeaveOnly/HeaveOnly.ino @@ -0,0 +1,118 @@ +#include +#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 → update reference (mm) e.g. R12.5 + // P,, → 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(); + } +} diff --git a/embedded/HeaveOnly/IndSensorLUT.cpp b/embedded/HeaveOnly/IndSensorLUT.cpp new file mode 120000 index 0000000..960360c --- /dev/null +++ b/embedded/HeaveOnly/IndSensorLUT.cpp @@ -0,0 +1 @@ +../lib/IndSensorLUT.cpp \ No newline at end of file diff --git a/embedded/HeaveOnly/IndSensorLUT.hpp b/embedded/HeaveOnly/IndSensorLUT.hpp new file mode 120000 index 0000000..e82eaa0 --- /dev/null +++ b/embedded/HeaveOnly/IndSensorLUT.hpp @@ -0,0 +1 @@ +../lib/IndSensorLUT.hpp \ No newline at end of file diff --git a/embedded/lib/HeaveController.cpp b/embedded/lib/HeaveController.cpp new file mode 100644 index 0000000..74333dc --- /dev/null +++ b/embedded/lib/HeaveController.cpp @@ -0,0 +1,125 @@ +#include "HeaveController.hpp" +#include +#include + +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 +} diff --git a/embedded/lib/HeaveController.hpp b/embedded/lib/HeaveController.hpp new file mode 100644 index 0000000..82a5454 --- /dev/null +++ b/embedded/lib/HeaveController.hpp @@ -0,0 +1,87 @@ +#ifndef HEAVE_CONTROLLER_HPP +#define HEAVE_CONTROLLER_HPP + +#include +#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 diff --git a/embedded/lib/IndSensorLUT.cpp b/embedded/lib/IndSensorLUT.cpp new file mode 100644 index 0000000..0564914 --- /dev/null +++ b/embedded/lib/IndSensorLUT.cpp @@ -0,0 +1,149 @@ +#include "IndSensorLUT.hpp" +#include +#include +#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); diff --git a/embedded/lib/IndSensorLUT.hpp b/embedded/lib/IndSensorLUT.hpp new file mode 100644 index 0000000..90e5bfb --- /dev/null +++ b/embedded/lib/IndSensorLUT.hpp @@ -0,0 +1,47 @@ +#ifndef IND_SENSOR_LUT_HPP +#define IND_SENSOR_LUT_HPP + +#include + +// 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 diff --git a/heave_plotter.py b/heave_plotter.py new file mode 100644 index 0000000..827fcd5 --- /dev/null +++ b/heave_plotter.py @@ -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 + PID (kp,ki,kd) → P,, +""" + +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() diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..f30e989 --- /dev/null +++ b/requirements.txt @@ -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