heave control + plotter

embedded/HeaveOnly/HeaveOnly.ino

@@ -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();
+  }
+}