Compare commits
6 Commits
e66488fa71
...
feature/le
| Author | SHA1 | Date | |
|---|---|---|---|
|
3a7f98a3d2
|
|||
|
6cc0cff069
|
|||
| 271cf7d3e8 | |||
| 2c81ddfd6b | |||
| e521ef8a2a | |||
| d0be1e9b0a |
104
CLAUDE.md
Normal file
104
CLAUDE.md
Normal file
@@ -0,0 +1,104 @@
|
||||
# CLAUDE.md
|
||||
|
||||
This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository.
|
||||
|
||||
## Build & Upload
|
||||
|
||||
This is a single-file Arduino sketch (`Gaugecontroller.ino`). Requires the **FastLED** library (`arduino-cli lib install FastLED`). Use the Arduino IDE or `arduino-cli`:
|
||||
|
||||
```bash
|
||||
# Compile (replace board/port as needed)
|
||||
arduino-cli compile --fqbn arduino:avr:mega Gaugecontroller.ino
|
||||
|
||||
# Upload
|
||||
arduino-cli upload -p /dev/ttyACM0 --fqbn arduino:avr:mega Gaugecontroller.ino
|
||||
```
|
||||
|
||||
Serial monitor: 115200 baud (`Serial` is both CMD_PORT and DEBUG_PORT).
|
||||
|
||||
## Switching serial ports (debug → production)
|
||||
|
||||
Two `#define`s at the top of `Gaugecontroller.ino` control where commands and debug output go:
|
||||
|
||||
```cpp
|
||||
#define CMD_PORT Serial // command channel (host sends SET, HOME, etc.)
|
||||
#define DEBUG_PORT Serial // diagnostic prints (homing, boot messages)
|
||||
```
|
||||
|
||||
**Debug / USB-only (default):** both point to `Serial` (the USB-CDC port). Connect via `minicom` or the Arduino IDE serial monitor at 115200 baud.
|
||||
|
||||
**Production (hardware UART):** change `CMD_PORT` to a hardware serial port so a host MCU or Raspberry Pi can drive it without occupying the USB port:
|
||||
|
||||
```cpp
|
||||
#define CMD_PORT Serial1 // TX1=pin18, RX1=pin19
|
||||
#define DEBUG_PORT Serial // keep USB for monitoring, or silence it (see below)
|
||||
```
|
||||
|
||||
Arduino Mega hardware UARTs:
|
||||
|
||||
| Port | TX pin | RX pin |
|
||||
|---------|--------|--------|
|
||||
| Serial1 | 18 | 19 |
|
||||
| Serial2 | 16 | 17 |
|
||||
| Serial3 | 14 | 15 |
|
||||
|
||||
`setup()` calls `DEBUG_PORT.begin(115200)` only. If `CMD_PORT` differs from `DEBUG_PORT` you must also begin it — add a second `begin` call in `setup()`:
|
||||
|
||||
```cpp
|
||||
CMD_PORT.begin(115200);
|
||||
```
|
||||
|
||||
**Silencing debug output entirely:** point `DEBUG_PORT` at a null stream, or wrap all `DEBUG_PORT` calls in an `#ifdef DEBUG` guard. The simplest option is to replace the define with a no-op object, but the easiest production approach is just to leave `DEBUG_PORT Serial` and ignore the USB output.
|
||||
|
||||
## Architecture
|
||||
|
||||
The sketch controls `GAUGE_COUNT` stepper-motor gauges using a trapezoidal velocity profile and a simple text serial protocol.
|
||||
|
||||
### Key data structures
|
||||
|
||||
- `GaugePins` — hardware pin mapping per gauge (dir, step, enable, active-high/low polarity flags, `ledCount`). Declared `constexpr` so `TOTAL_LEDS` can be computed from it at compile time. Configured in the `gaugePins[]` array at the top.
|
||||
- `Gauge` — per-gauge runtime state: position, target, velocity, accel, homing state machine, sweep mode.
|
||||
|
||||
### Motion control (`updateGauge`)
|
||||
|
||||
Each call to `updateGauge(id)` in `loop()` computes `dt` since last call and updates velocity using a braking-distance check to produce smooth trapezoidal motion. Steps are accumulated as floating-point and emitted via `doStep` when the accumulator crosses ±1.
|
||||
|
||||
### Homing sequence (`updateHoming`)
|
||||
|
||||
State machine: `HS_START → HS_BACKING → HS_SETTLE → HS_DONE → HS_IDLE`.
|
||||
Backs up `homingBackoffSteps` at `homingSpeed`, waits 100 ms settle, then declares `currentPos = 0`. No physical end-stop is used; homing is purely time/step-count based.
|
||||
|
||||
### Sweep mode
|
||||
|
||||
When `sweepEnabled`, `updateSweepTarget` bounces `targetPos` between `minPos` and `maxPos` autonomously.
|
||||
|
||||
### LED strip
|
||||
|
||||
One shared WS2812B strip is driven from `LED_DATA_PIN` (default 6). Each gauge owns a contiguous segment of the strip; `gaugePins[i].ledCount` sets the segment length (0 = no LEDs). `TOTAL_LEDS` is computed at compile time via `constexpr sumLedCounts()` — no manual constant to keep in sync. Per-gauge offsets into the flat `leds[]` array are computed once in `setup()` into `gaugeLedOffset[]`. `FastLED.show()` is called immediately after each `LED` command.
|
||||
|
||||
### Serial command protocol
|
||||
|
||||
Commands arrive as newline-terminated ASCII lines. Each `parse*` function in `processLine` handles one command family:
|
||||
|
||||
| Command | Syntax | Effect |
|
||||
|---|---|---|
|
||||
| `SET` | `SET <id> <pos>` | Move gauge to absolute step position |
|
||||
| `SPEED` | `SPEED <id> <steps/s>` | Set max speed |
|
||||
| `ACCEL` | `ACCEL <id> <steps/s²>` | Set acceleration |
|
||||
| `ENABLE` | `ENABLE <id> <0\|1>` | Enable/disable driver output |
|
||||
| `ZERO` | `ZERO <id>` | Mark current position as home without moving |
|
||||
| `HOME` | `HOME <id>` / `HOMEALL` | Run homing sequence |
|
||||
| `SWEEP` | `SWEEP <id> <accel> <speed>` | Start sweep (0/0 stops) |
|
||||
| `POS?` | `POS?` | Query all gauges: `POS <id> <cur> <tgt> <homed> <homingState> <sweep>` |
|
||||
| `LED` | `LED <id> <idx> <r> <g> <b>` | Set one LED (0-based index within gauge segment) to RGB colour (0–255 each); `<idx>` may be a range `N-M` to set LEDs N through M in one command |
|
||||
| `LED?` | `LED?` | Query all LEDs: one `LED <id> <idx> <r> <g> <b>` line per LED, then `OK` |
|
||||
| `PING` | `PING` | Responds `PONG` |
|
||||
|
||||
All commands reply `OK` or `ERR BAD_ID` / `ERR BAD_CMD` etc.
|
||||
|
||||
### Adding gauges
|
||||
|
||||
1. Increment `GAUGE_COUNT`.
|
||||
2. Add a `constexpr GaugePins` entry to `gaugePins[]` (including `ledCount`).
|
||||
3. Tune `maxPos` and `homingBackoffSteps` in the corresponding `Gauge` default or at runtime.
|
||||
4. `TOTAL_LEDS` and `gaugeLedOffset[]` update automatically — no manual changes needed.
|
||||
620
Gaugecontroller/Gaugecontroller.ino
Normal file
620
Gaugecontroller/Gaugecontroller.ino
Normal file
@@ -0,0 +1,620 @@
|
||||
#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 Serial
|
||||
#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 = 3610; // 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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user