119 lines
3.7 KiB
C++
119 lines
3.7 KiB
C++
#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();
|
|
}
|
|
}
|