621 lines
14 KiB
C++
621 lines
14 KiB
C++
#include <Arduino.h>
|
|
#include <math.h>
|
|
#include <FastLED.h>
|
|
|
|
static const uint8_t GAUGE_COUNT = 2;
|
|
|
|
// LED strip — one shared WS2812B strip, segmented per gauge.
|
|
// Set LED_DATA_PIN to the digital pin driving the strip data line.
|
|
// TOTAL_LEDS is computed automatically from gaugePins[].ledCount.
|
|
static const uint8_t LED_DATA_PIN = 22;
|
|
|
|
// For now: commands come over USB serial
|
|
#define CMD_PORT Serial1
|
|
#define DEBUG_PORT Serial
|
|
|
|
struct GaugePins {
|
|
uint8_t dirPin;
|
|
uint8_t stepPin;
|
|
int8_t enablePin; // -1 if unused
|
|
bool dirInverted;
|
|
bool stepActiveHigh;
|
|
bool enableActiveLow;
|
|
uint8_t ledCount; // WS2812B LEDs on this gauge's strip segment (0 if none)
|
|
};
|
|
|
|
constexpr GaugePins gaugePins[GAUGE_COUNT] = {
|
|
// dir, step, en, dirInv, stepHigh, enActiveLow, leds
|
|
{50, 51, -1, false, true, true, 6}, // Gauge 0
|
|
{8, 9, -1, true, true, true, 6}, // Gauge 1
|
|
};
|
|
|
|
constexpr uint8_t sumLedCounts(uint8_t i = 0) {
|
|
return i >= GAUGE_COUNT ? 0 : gaugePins[i].ledCount + sumLedCounts(i + 1);
|
|
}
|
|
static const uint8_t TOTAL_LEDS = sumLedCounts();
|
|
|
|
enum HomingState : uint8_t {
|
|
HS_IDLE,
|
|
HS_START,
|
|
HS_BACKING,
|
|
HS_SETTLE,
|
|
HS_DONE
|
|
};
|
|
|
|
struct Gauge {
|
|
long currentPos = 0;
|
|
long targetPos = 0;
|
|
|
|
long minPos = 0;
|
|
long maxPos = 3780; // adjust to your usable travel
|
|
long homingBackoffSteps = 3700; // should exceed reverse travel slightly
|
|
|
|
float velocity = 0.0f;
|
|
float maxSpeed = 5000.0f;
|
|
float accel = 6000.0f;
|
|
float homingSpeed = 500.0f;
|
|
|
|
float stepAccumulator = 0.0f;
|
|
unsigned long lastUpdateMicros = 0;
|
|
|
|
bool enabled = true;
|
|
bool homed = false;
|
|
|
|
HomingState homingState = HS_IDLE;
|
|
long homingStepsRemaining = 0;
|
|
unsigned long homingLastStepMicros = 0;
|
|
unsigned long homingStateStartMs = 0;
|
|
|
|
bool sweepEnabled = false;
|
|
bool sweepTowardMax = true;
|
|
};
|
|
|
|
Gauge gauges[GAUGE_COUNT];
|
|
String rxLine;
|
|
|
|
CRGB leds[TOTAL_LEDS];
|
|
uint8_t gaugeLedOffset[GAUGE_COUNT];
|
|
|
|
void sendReply(const String& s) {
|
|
CMD_PORT.println(s);
|
|
}
|
|
|
|
float absf(float x) {
|
|
return (x < 0.0f) ? -x : x;
|
|
}
|
|
|
|
void setEnable(uint8_t id, bool en) {
|
|
if (id >= GAUGE_COUNT) return;
|
|
gauges[id].enabled = en;
|
|
|
|
int8_t pin = gaugePins[id].enablePin;
|
|
if (pin < 0) return;
|
|
|
|
bool level = gaugePins[id].enableActiveLow ? !en : en;
|
|
digitalWrite(pin, level ? HIGH : LOW);
|
|
}
|
|
|
|
void setDir(uint8_t id, bool forward) {
|
|
bool level = gaugePins[id].dirInverted ? !forward : forward;
|
|
digitalWrite(gaugePins[id].dirPin, level ? HIGH : LOW);
|
|
}
|
|
|
|
void pulseStep(uint8_t id) {
|
|
bool active = gaugePins[id].stepActiveHigh;
|
|
digitalWrite(gaugePins[id].stepPin, active ? HIGH : LOW);
|
|
delayMicroseconds(4);
|
|
digitalWrite(gaugePins[id].stepPin, active ? LOW : HIGH);
|
|
}
|
|
|
|
void doStep(uint8_t id, int dir, bool allowPastMin = false) {
|
|
Gauge& g = gauges[id];
|
|
if (!g.enabled) return;
|
|
|
|
if (dir > 0) {
|
|
if (g.currentPos >= g.maxPos) return;
|
|
setDir(id, true);
|
|
pulseStep(id);
|
|
g.currentPos++;
|
|
} else if (dir < 0) {
|
|
if (!allowPastMin && g.currentPos <= g.minPos) return;
|
|
setDir(id, false);
|
|
pulseStep(id);
|
|
g.currentPos--;
|
|
}
|
|
}
|
|
|
|
void requestHome(uint8_t id) {
|
|
if (id >= GAUGE_COUNT) return;
|
|
Gauge& g = gauges[id];
|
|
g.homingState = HS_START;
|
|
g.homed = false;
|
|
g.velocity = 0.0f;
|
|
g.stepAccumulator = 0.0f;
|
|
g.sweepEnabled = false;
|
|
}
|
|
|
|
void requestHomeAll() {
|
|
for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
|
|
requestHome(i);
|
|
}
|
|
}
|
|
|
|
void updateHoming(uint8_t id) {
|
|
Gauge& g = gauges[id];
|
|
unsigned long nowUs = micros();
|
|
unsigned long nowMs = millis();
|
|
|
|
switch (g.homingState) {
|
|
case HS_IDLE:
|
|
return;
|
|
|
|
case HS_START:
|
|
g.velocity = 0.0f;
|
|
g.stepAccumulator = 0.0f;
|
|
g.homingStepsRemaining = g.homingBackoffSteps;
|
|
g.homingLastStepMicros = nowUs;
|
|
g.homingState = HS_BACKING;
|
|
break;
|
|
|
|
case HS_BACKING: {
|
|
float intervalUs = 1000000.0f / g.homingSpeed;
|
|
if ((nowUs - g.homingLastStepMicros) >= intervalUs) {
|
|
g.homingLastStepMicros = nowUs;
|
|
|
|
if (g.homingStepsRemaining > 0) {
|
|
doStep(id, -1, true);
|
|
g.homingStepsRemaining--;
|
|
} else {
|
|
g.homingState = HS_SETTLE;
|
|
g.homingStateStartMs = nowMs;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
case HS_SETTLE:
|
|
if (nowMs - g.homingStateStartMs >= 100) {
|
|
g.currentPos = 0;
|
|
g.targetPos = 0;
|
|
g.velocity = 0.0f;
|
|
g.stepAccumulator = 0.0f;
|
|
g.homed = true;
|
|
g.homingState = HS_DONE;
|
|
|
|
DEBUG_PORT.print("HOMED ");
|
|
DEBUG_PORT.println(id);
|
|
}
|
|
break;
|
|
|
|
case HS_DONE:
|
|
g.homingState = HS_IDLE;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void updateSweepTarget(uint8_t id) {
|
|
Gauge& g = gauges[id];
|
|
if (!g.sweepEnabled || !g.homed || g.homingState != HS_IDLE) return;
|
|
|
|
if (g.sweepTowardMax) {
|
|
g.targetPos = g.maxPos;
|
|
if (g.currentPos >= g.maxPos && absf(g.velocity) < 1.0f) {
|
|
g.sweepTowardMax = false;
|
|
g.targetPos = g.minPos;
|
|
}
|
|
} else {
|
|
g.targetPos = g.minPos;
|
|
if (g.currentPos <= g.minPos && absf(g.velocity) < 1.0f) {
|
|
g.sweepTowardMax = true;
|
|
g.targetPos = g.maxPos;
|
|
}
|
|
}
|
|
}
|
|
|
|
void updateGauge(uint8_t id) {
|
|
Gauge& g = gauges[id];
|
|
|
|
if (g.homingState != HS_IDLE) {
|
|
updateHoming(id);
|
|
return;
|
|
}
|
|
|
|
if (!g.homed) return;
|
|
|
|
if (g.sweepEnabled) {
|
|
updateSweepTarget(id);
|
|
}
|
|
|
|
unsigned long now = micros();
|
|
if (g.lastUpdateMicros == 0) {
|
|
g.lastUpdateMicros = now;
|
|
return;
|
|
}
|
|
|
|
float dt = (now - g.lastUpdateMicros) / 1000000.0f;
|
|
g.lastUpdateMicros = now;
|
|
|
|
if (dt <= 0.0f || dt > 0.1f) return;
|
|
|
|
long error = g.targetPos - g.currentPos;
|
|
|
|
if (error == 0 && absf(g.velocity) < 0.01f) {
|
|
g.velocity = 0.0f;
|
|
g.stepAccumulator = 0.0f;
|
|
return;
|
|
}
|
|
|
|
float dir = (error > 0) ? 1.0f : (error < 0 ? -1.0f : 0.0f);
|
|
float brakingDistance = (g.velocity * g.velocity) / (2.0f * g.accel + 0.0001f);
|
|
|
|
if ((float)labs(error) <= brakingDistance) {
|
|
if (g.velocity > 0.0f) {
|
|
g.velocity -= g.accel * dt;
|
|
if (g.velocity < 0.0f) g.velocity = 0.0f;
|
|
} else if (g.velocity < 0.0f) {
|
|
g.velocity += g.accel * dt;
|
|
if (g.velocity > 0.0f) g.velocity = 0.0f;
|
|
}
|
|
} else {
|
|
g.velocity += dir * g.accel * dt;
|
|
if (g.velocity > g.maxSpeed) g.velocity = g.maxSpeed;
|
|
if (g.velocity < -g.maxSpeed) g.velocity = -g.maxSpeed;
|
|
}
|
|
|
|
if (fabs(g.velocity) < 0.01f && error != 0) {
|
|
g.velocity = dir * 5.0f;
|
|
}
|
|
|
|
g.stepAccumulator += g.velocity * dt;
|
|
|
|
while (g.stepAccumulator >= 1.0f) {
|
|
if (g.currentPos == g.targetPos) {
|
|
g.stepAccumulator = 0.0f;
|
|
g.velocity = 0.0f;
|
|
break;
|
|
}
|
|
|
|
doStep(id, +1, false);
|
|
g.stepAccumulator -= 1.0f;
|
|
|
|
if (g.currentPos >= g.maxPos) {
|
|
g.currentPos = g.maxPos;
|
|
g.targetPos = g.maxPos;
|
|
g.velocity = 0.0f;
|
|
g.stepAccumulator = 0.0f;
|
|
break;
|
|
}
|
|
}
|
|
|
|
while (g.stepAccumulator <= -1.0f) {
|
|
if (g.currentPos == g.targetPos) {
|
|
g.stepAccumulator = 0.0f;
|
|
g.velocity = 0.0f;
|
|
break;
|
|
}
|
|
|
|
doStep(id, -1, false);
|
|
g.stepAccumulator += 1.0f;
|
|
|
|
if (g.currentPos <= g.minPos) {
|
|
g.currentPos = g.minPos;
|
|
g.targetPos = g.minPos;
|
|
g.velocity = 0.0f;
|
|
g.stepAccumulator = 0.0f;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool parseSet(const String& line) {
|
|
int id;
|
|
long pos;
|
|
if (sscanf(line.c_str(), "SET %d %ld", &id, &pos) == 2) {
|
|
if (id < 0 || id >= GAUGE_COUNT) {
|
|
sendReply("ERR BAD_ID");
|
|
return true;
|
|
}
|
|
|
|
Gauge& g = gauges[id];
|
|
pos = constrain(pos, g.minPos, g.maxPos);
|
|
g.targetPos = pos;
|
|
g.sweepEnabled = false;
|
|
sendReply("OK");
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool parseSpeed(const String& line) {
|
|
int firstSpace = line.indexOf(' ');
|
|
int secondSpace = line.indexOf(' ', firstSpace + 1);
|
|
if (firstSpace < 0 || secondSpace < 0) return false;
|
|
if (line.substring(0, firstSpace) != "SPEED") return false;
|
|
|
|
int id = line.substring(firstSpace + 1, secondSpace).toInt();
|
|
float speed = line.substring(secondSpace + 1).toFloat();
|
|
|
|
if (id < 0 || id >= GAUGE_COUNT) {
|
|
sendReply("ERR BAD_ID");
|
|
return true;
|
|
}
|
|
if (speed <= 0.0f) {
|
|
sendReply("ERR BAD_SPEED");
|
|
return true;
|
|
}
|
|
|
|
gauges[id].maxSpeed = speed;
|
|
sendReply("OK");
|
|
return true;
|
|
}
|
|
|
|
bool parseAccel(const String& line) {
|
|
int firstSpace = line.indexOf(' ');
|
|
int secondSpace = line.indexOf(' ', firstSpace + 1);
|
|
if (firstSpace < 0 || secondSpace < 0) return false;
|
|
if (line.substring(0, firstSpace) != "ACCEL") return false;
|
|
|
|
int id = line.substring(firstSpace + 1, secondSpace).toInt();
|
|
float accel = line.substring(secondSpace + 1).toFloat();
|
|
|
|
if (id < 0 || id >= GAUGE_COUNT) {
|
|
sendReply("ERR BAD_ID");
|
|
return true;
|
|
}
|
|
if (accel <= 0.0f) {
|
|
sendReply("ERR BAD_ACCEL");
|
|
return true;
|
|
}
|
|
|
|
gauges[id].accel = accel;
|
|
sendReply("OK");
|
|
return true;
|
|
}
|
|
|
|
bool parseEnable(const String& line) {
|
|
int id, en;
|
|
if (sscanf(line.c_str(), "ENABLE %d %d", &id, &en) == 2) {
|
|
if (id < 0 || id >= GAUGE_COUNT) {
|
|
sendReply("ERR BAD_ID");
|
|
return true;
|
|
}
|
|
|
|
setEnable(id, en != 0);
|
|
sendReply("OK");
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool parseZero(const String& line) {
|
|
int id;
|
|
if (sscanf(line.c_str(), "ZERO %d", &id) == 1) {
|
|
if (id < 0 || id >= GAUGE_COUNT) {
|
|
sendReply("ERR BAD_ID");
|
|
return true;
|
|
}
|
|
|
|
Gauge& g = gauges[id];
|
|
g.currentPos = 0;
|
|
g.targetPos = 0;
|
|
g.velocity = 0.0f;
|
|
g.stepAccumulator = 0.0f;
|
|
g.homed = true;
|
|
g.sweepEnabled = false;
|
|
sendReply("OK");
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool parseHome(const String& line) {
|
|
int id;
|
|
if (sscanf(line.c_str(), "HOME %d", &id) == 1) {
|
|
if (id < 0 || id >= GAUGE_COUNT) {
|
|
sendReply("ERR BAD_ID");
|
|
return true;
|
|
}
|
|
|
|
requestHome(id);
|
|
sendReply("OK");
|
|
return true;
|
|
}
|
|
|
|
if (line == "HOMEALL") {
|
|
requestHomeAll();
|
|
sendReply("OK");
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
bool parseSweep(const String& line) {
|
|
int firstSpace = line.indexOf(' ');
|
|
int secondSpace = line.indexOf(' ', firstSpace + 1);
|
|
int thirdSpace = line.indexOf(' ', secondSpace + 1);
|
|
|
|
if (firstSpace < 0 || secondSpace < 0 || thirdSpace < 0) return false;
|
|
if (line.substring(0, firstSpace) != "SWEEP") return false;
|
|
|
|
int id = line.substring(firstSpace + 1, secondSpace).toInt();
|
|
float accel = line.substring(secondSpace + 1, thirdSpace).toFloat();
|
|
float speed = line.substring(thirdSpace + 1).toFloat();
|
|
|
|
if (id < 0 || id >= GAUGE_COUNT) {
|
|
sendReply("ERR BAD_ID");
|
|
return true;
|
|
}
|
|
|
|
Gauge& g = gauges[id];
|
|
|
|
if (accel <= 0.0f || speed <= 0.0f) {
|
|
g.sweepEnabled = false;
|
|
g.velocity = 0.0f;
|
|
g.stepAccumulator = 0.0f;
|
|
sendReply("OK");
|
|
return true;
|
|
}
|
|
|
|
g.accel = accel;
|
|
g.maxSpeed = speed;
|
|
g.sweepEnabled = true;
|
|
g.sweepTowardMax = true;
|
|
g.targetPos = g.maxPos;
|
|
sendReply("OK");
|
|
return true;
|
|
}
|
|
|
|
bool parsePosQuery(const String& line) {
|
|
if (line == "POS?") {
|
|
for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
|
|
CMD_PORT.print("POS ");
|
|
CMD_PORT.print(i);
|
|
CMD_PORT.print(' ');
|
|
CMD_PORT.print(gauges[i].currentPos);
|
|
CMD_PORT.print(' ');
|
|
CMD_PORT.print(gauges[i].targetPos);
|
|
CMD_PORT.print(' ');
|
|
CMD_PORT.print(gauges[i].homed ? 1 : 0);
|
|
CMD_PORT.print(' ');
|
|
CMD_PORT.print((int)gauges[i].homingState);
|
|
CMD_PORT.print(' ');
|
|
CMD_PORT.println(gauges[i].sweepEnabled ? 1 : 0);
|
|
}
|
|
sendReply("OK");
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool parsePing(const String& line) {
|
|
if (line == "PING") {
|
|
sendReply("PONG");
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool parseLedQuery(const String& line) {
|
|
if (line == "LED?") {
|
|
for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
|
|
for (uint8_t j = 0; j < gaugePins[i].ledCount; j++) {
|
|
const CRGB& c = leds[gaugeLedOffset[i] + j];
|
|
CMD_PORT.print("LED ");
|
|
CMD_PORT.print(i);
|
|
CMD_PORT.print(' ');
|
|
CMD_PORT.print(j);
|
|
CMD_PORT.print(' ');
|
|
CMD_PORT.print(c.r);
|
|
CMD_PORT.print(' ');
|
|
CMD_PORT.print(c.g);
|
|
CMD_PORT.print(' ');
|
|
CMD_PORT.println(c.b);
|
|
}
|
|
}
|
|
sendReply("OK");
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool parseLed(const String& line) {
|
|
int id, r, g, b;
|
|
char idxToken[16];
|
|
if (sscanf(line.c_str(), "LED %d %15s %d %d %d", &id, idxToken, &r, &g, &b) == 5) {
|
|
if (id < 0 || id >= GAUGE_COUNT) { sendReply("ERR BAD_ID"); return true; }
|
|
char* dash = strchr(idxToken, '-');
|
|
int idxFirst = atoi(idxToken);
|
|
int idxLast = dash ? atoi(dash + 1) : idxFirst;
|
|
if (idxFirst < 0 || idxLast >= gaugePins[id].ledCount || idxFirst > idxLast) {
|
|
sendReply("ERR BAD_IDX"); return true;
|
|
}
|
|
CRGB color(constrain(r, 0, 255), constrain(g, 0, 255), constrain(b, 0, 255));
|
|
for (int i = idxFirst; i <= idxLast; i++)
|
|
leds[gaugeLedOffset[id] + i] = color;
|
|
FastLED.show();
|
|
sendReply("OK");
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void processLine(const String& line) {
|
|
if (parseSet(line)) return;
|
|
if (parseSpeed(line)) return;
|
|
if (parseAccel(line)) return;
|
|
if (parseEnable(line)) return;
|
|
if (parseZero(line)) return;
|
|
if (parseHome(line)) return;
|
|
if (parseSweep(line)) return;
|
|
if (parsePosQuery(line)) return;
|
|
if (parseLedQuery(line)) return;
|
|
if (parseLed(line)) return;
|
|
if (parsePing(line)) return;
|
|
|
|
sendReply("ERR BAD_CMD");
|
|
}
|
|
|
|
void readCommands() {
|
|
while (CMD_PORT.available()) {
|
|
char c = (char)CMD_PORT.read();
|
|
|
|
if (c == '\n') {
|
|
rxLine.trim();
|
|
if (rxLine.length() > 0) {
|
|
processLine(rxLine);
|
|
}
|
|
rxLine = "";
|
|
} else if (c != '\r') {
|
|
rxLine += c;
|
|
if (rxLine.length() > 120) {
|
|
rxLine = "";
|
|
sendReply("ERR TOO_LONG");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void setup() {
|
|
DEBUG_PORT.begin(115200);
|
|
DEBUG_PORT.println("Gauge controller booting");
|
|
|
|
for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
|
|
pinMode(gaugePins[i].dirPin, OUTPUT);
|
|
pinMode(gaugePins[i].stepPin, OUTPUT);
|
|
|
|
digitalWrite(gaugePins[i].dirPin, LOW);
|
|
digitalWrite(gaugePins[i].stepPin, gaugePins[i].stepActiveHigh ? LOW : HIGH);
|
|
|
|
if (gaugePins[i].enablePin >= 0) {
|
|
pinMode(gaugePins[i].enablePin, OUTPUT);
|
|
setEnable(i, true);
|
|
}
|
|
|
|
gauges[i].lastUpdateMicros = micros();
|
|
}
|
|
|
|
// Compute per-gauge LED offsets and initialise the strip.
|
|
uint8_t ledOff = 0;
|
|
for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
|
|
gaugeLedOffset[i] = ledOff;
|
|
ledOff += gaugePins[i].ledCount;
|
|
}
|
|
FastLED.addLeds<WS2812B, LED_DATA_PIN, GRB>(leds, TOTAL_LEDS);
|
|
FastLED.setBrightness(255);
|
|
FastLED.show();
|
|
|
|
requestHomeAll();
|
|
|
|
DEBUG_PORT.println("READY");
|
|
sendReply("READY");
|
|
}
|
|
|
|
void loop() {
|
|
readCommands();
|
|
|
|
for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
|
|
updateGauge(i);
|
|
}
|
|
}
|