Code commented, Serial speed moved into constant

CLAUDE.md

@@ -4,37 +4,47 @@ This file provides guidance to Claude Code (claude.ai/code) when working with co
 
 ## 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`:
+Main firmware lives in `Gaugecontroller/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
+arduino-cli compile --fqbn arduino:avr:mega Gaugecontroller
 
 # Upload
-arduino-cli upload -p /dev/ttyACM0 --fqbn arduino:avr:mega Gaugecontroller.ino
+arduino-cli upload -p /dev/ttyACM0 --fqbn arduino:avr:mega Gaugecontroller
 ```
 
-Serial monitor: 115200 baud (`Serial` is both CMD_PORT and DEBUG_PORT).
+Current default serial setup: `CMD_PORT` and `DEBUG_PORT` both point to `Serial1` at 38400 baud.
 
 ## 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)
+#define CMD_PORT   Serial1  // command channel (host sends SET, HOME, etc.)
+#define DEBUG_PORT Serial1  // 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.
+**Current default:** both point to `Serial1`, so command and debug traffic share Mega pins TX1=18 / RX1=19 at 38400 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:
+**USB-only debug setup:** point both defines back at `Serial` if you want to talk to the sketch over the Arduino USB port instead:
 
 ```cpp
-#define CMD_PORT   Serial1  // TX1=pin18, RX1=pin19
-#define DEBUG_PORT Serial   // keep USB for monitoring, or silence it (see below)
+#define CMD_PORT   Serial
+#define DEBUG_PORT Serial
 ```
 
-Arduino Mega hardware UARTs:
+At that point the matching `begin()` call in `setup()` also needs to use the same baud rate you expect on the host side.
+
+**Split command/debug ports:** if `CMD_PORT` and `DEBUG_PORT` do not point to the same serial port, `setup()` must initialise both. Right now it only calls:
+
+```cpp
+DEBUG_PORT.begin(38400);
+```
+
+If you split them, add a second `CMD_PORT.begin(...)` call.
+
+Arduino Mega hardware UARTs for reference:
 
 | Port    | TX pin | RX pin |
 |---------|--------|--------|
@@ -42,14 +52,6 @@ Arduino Mega hardware UARTs:
 | 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.
@@ -74,7 +76,7 @@ When `sweepEnabled`, `updateSweepTarget` bounces `targetPos` between `minPos` an
 
 ### 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.
+One shared WS2812B strip is driven from `LED_DATA_PIN` (currently 22). 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[]`. LED commands and effects mark the strip dirty, and `FastLED.show()` is called once per main-loop iteration if anything changed.
 
 ### Serial command protocol
 

Gaugecontroller/Gaugecontroller.ino

@@ -2,31 +2,31 @@
 #include <math.h>
 #include <FastLED.h>
 
-static const uint8_t GAUGE_COUNT = 2;
+static const uint8_t GAUGE_COUNT = 3;
 
-// 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.
+// One shared WS2812B strip, split into per-gauge segments.
 static const uint8_t LED_DATA_PIN = 22;
 
-// For now: commands come over USB serial
+// For now, command and debug traffic share the same serial port.
 #define CMD_PORT Serial1
 #define DEBUG_PORT Serial1
+static const unsigned long SERIAL_BAUD = 38400;
 
 struct GaugePins {
   uint8_t dirPin;
   uint8_t stepPin;
-  int8_t enablePin;          // -1 if unused
+  int8_t enablePin;          // -1 means there is no enable pin
   bool dirInverted;
   bool stepActiveHigh;
   bool enableActiveLow;
-  uint8_t ledCount;          // WS2812B LEDs on this gauge's strip segment (0 if none)
+  uint8_t ledCount;          // LEDs assigned to this gauge
 };
 
 constexpr GaugePins gaugePins[GAUGE_COUNT] = {
   // dir, step, en, dirInv, stepHigh, enActiveLow, leds
   {50, 51, -1, false, true, true, 7},   // Gauge 0
   {8,  9,  -1, true,  true, true, 7},   // Gauge 1
+  {52, 53, -1, false, true, true, 7},   // Gauge 2
 };
 
 constexpr uint8_t sumLedCounts(uint8_t i = 0) {
@@ -47,8 +47,8 @@ struct Gauge {
   long targetPos = 0;
 
   long minPos = 0;
-  long maxPos = 3780;             // adjust to your usable travel
-  long homingBackoffSteps = 3700; // should exceed reverse travel slightly
+  long maxPos = 3780;
+  long homingBackoffSteps = 3800; // Deliberately a touch past full reverse travel.
 
   float velocity = 0.0f;
   float maxSpeed = 5000.0f;
@@ -77,14 +77,11 @@ struct BlinkState {
   LedFx         fx          = FX_BLINK;
   CRGB          onColor;
   unsigned long lastMs      = 0;
-  // FX_BLINK
   uint16_t      onMs        = 500;
   uint16_t      offMs       = 500;
   bool          currentlyOn = false;
-  // FX_BREATHE: smooth triangle-wave fade
   uint16_t      periodMs    = 2000;
   uint16_t      cyclePos    = 0;
-  // FX_DFLASH: two quick flashes then pause
   uint8_t       dphase      = 0;
 };
 
@@ -96,14 +93,65 @@ uint8_t gaugeLedOffset[GAUGE_COUNT];
 BlinkState blinkState[TOTAL_LEDS];
 bool ledsDirty = false;
 
+// Sends one-line command replies back over the control port.
+//
+// Serial protocol summary.
+//
+// Host -> controller commands (newline-terminated ASCII):
+//   SET <id> <pos>
+//   SPEED <id> <steps_per_s>
+//   ACCEL <id> <steps_per_s2>
+//   ENABLE <id> <0|1>
+//   ZERO <id>
+//   HOME <id>
+//   HOMEALL
+//   SWEEP <id> <accel> <speed>
+//   POS?
+//   LED?
+//   LED <id> <idx|a-b> <r> <g> <b>
+//   BLINK <id> <idx|a-b> <on_ms> <off_ms> [<r> <g> <b>]
+//   BREATHE <id> <idx|a-b> <period_ms> <r> <g> <b>
+//   DFLASH <id> <idx|a-b> <r> <g> <b>
+//   PING
+//
+// Controller -> host replies / events:
+//   READY
+//     Sent once from setup() after boot completes.
+//   OK
+//     Sent after a valid mutating command, and after POS?/LED? once all data lines
+//     for that query have been emitted.
+//   PONG
+//     Sent in response to PING.
+//   ERR BAD_CMD
+//     Sent when a complete line matches no parser.
+//   ERR TOO_LONG
+//     Sent when an input line exceeds the receive buffer limit.
+//   ERR BAD_ID
+//     Sent by commands that take a gauge id when the id is outside 0..GAUGE_COUNT-1.
+//   ERR BAD_SPEED
+//     Sent by SPEED when the requested speed is <= 0.
+//   ERR BAD_ACCEL
+//     Sent by ACCEL when the requested acceleration is <= 0.
+//   ERR BAD_IDX
+//     Sent by LED/BLINK/BREATHE/DFLASH when an LED index or range is invalid.
+//   ERR BAD_TIME
+//     Sent by BLINK/BREATHE when the timing parameter is invalid.
+//   POS <id> <currentPos> <targetPos> <homed> <homingState> <sweepEnabled>
+//     Emitted once per gauge before the trailing OK reply to POS?.
+//   LED <id> <idx> <r> <g> <b>
+//     Emitted once per configured LED before the trailing OK reply to LED?.
+//   HOMED <id>
+//     Debug event printed on DEBUG_PORT when a homing sequence settles successfully.
 void sendReply(const String& s) {
   CMD_PORT.println(s);
 }
 
+// Tiny float absolute-value helper to avoid dragging more machinery into the sketch.
 float absf(float x) {
   return (x < 0.0f) ? -x : x;
 }
 
+// Updates the cached enable state and toggles the hardware pin if one exists.
 void setEnable(uint8_t id, bool en) {
   if (id >= GAUGE_COUNT) return;
   gauges[id].enabled = en;
@@ -115,11 +163,13 @@ void setEnable(uint8_t id, bool en) {
   digitalWrite(pin, level ? HIGH : LOW);
 }
 
+// Applies the logical direction after accounting for per-gauge inversion.
 void setDir(uint8_t id, bool forward) {
   bool level = gaugePins[id].dirInverted ? !forward : forward;
   digitalWrite(gaugePins[id].dirPin, level ? HIGH : LOW);
 }
 
+// Emits one step pulse with the polarity expected by the driver.
 void pulseStep(uint8_t id) {
   bool active = gaugePins[id].stepActiveHigh;
   digitalWrite(gaugePins[id].stepPin, active ? HIGH : LOW);
@@ -127,6 +177,7 @@ void pulseStep(uint8_t id) {
   digitalWrite(gaugePins[id].stepPin, active ? LOW : HIGH);
 }
 
+// Moves the motor by one step if the requested direction is still within allowed travel.
 void doStep(uint8_t id, int dir, bool allowPastMin = false) {
   Gauge& g = gauges[id];
   if (!g.enabled) return;
@@ -144,6 +195,7 @@ void doStep(uint8_t id, int dir, bool allowPastMin = false) {
   }
 }
 
+// Arms the homing state machine for one gauge and clears any in-flight motion.
 void requestHome(uint8_t id) {
   if (id >= GAUGE_COUNT) return;
   Gauge& g = gauges[id];
@@ -154,12 +206,14 @@ void requestHome(uint8_t id) {
   g.sweepEnabled = false;
 }
 
+// Starts the same homing sequence on every configured gauge.
 void requestHomeAll() {
   for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
     requestHome(i);
   }
 }
 
+// Advances the simple homing state machine until the gauge is parked at logical zero.
 void updateHoming(uint8_t id) {
   Gauge& g = gauges[id];
   unsigned long nowUs = micros();
@@ -170,6 +224,7 @@ void updateHoming(uint8_t id) {
       return;
 
     case HS_START:
+      // No endstop here; homing just walks back far enough to hit the hard stop.
       g.velocity = 0.0f;
       g.stepAccumulator = 0.0f;
       g.homingStepsRemaining = g.homingBackoffSteps;
@@ -213,6 +268,7 @@ void updateHoming(uint8_t id) {
   }
 }
 
+// Flips the sweep destination when the gauge has settled at either end of travel.
 void updateSweepTarget(uint8_t id) {
   Gauge& g = gauges[id];
   if (!g.sweepEnabled || !g.homed || g.homingState != HS_IDLE) return;
@@ -232,6 +288,7 @@ void updateSweepTarget(uint8_t id) {
   }
 }
 
+// Runs one gauge worth of motion control, including homing and optional sweeping.
 void updateGauge(uint8_t id) {
   Gauge& g = gauges[id];
 
@@ -266,6 +323,7 @@ void updateGauge(uint8_t id) {
   }
 
   float dir = (error > 0) ? 1.0f : (error < 0 ? -1.0f : 0.0f);
+  // Basic trapezoidal profile: brake if the remaining travel is shorter than the stop distance.
   float brakingDistance = (g.velocity * g.velocity) / (2.0f * g.accel + 0.0001f);
 
   if ((float)labs(error) <= brakingDistance) {
@@ -286,6 +344,7 @@ void updateGauge(uint8_t id) {
     g.velocity = dir * 5.0f;
   }
 
+  // Integrate fractional steps until there is enough to emit a real pulse.
   g.stepAccumulator += g.velocity * dt;
 
   while (g.stepAccumulator >= 1.0f) {
@@ -327,6 +386,8 @@ void updateGauge(uint8_t id) {
   }
 }
 
+// Parses `SET <id> <pos>` and updates the target position.
+// Replies: `OK`, `ERR BAD_ID`.
 bool parseSet(const String& line) {
   int id;
   long pos;
@@ -346,6 +407,8 @@ bool parseSet(const String& line) {
   return false;
 }
 
+// Parses `SPEED <id> <speed>` and updates the max step rate.
+// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_SPEED`.
 bool parseSpeed(const String& line) {
   int firstSpace = line.indexOf(' ');
   int secondSpace = line.indexOf(' ', firstSpace + 1);
@@ -369,6 +432,8 @@ bool parseSpeed(const String& line) {
   return true;
 }
 
+// Parses `ACCEL <id> <accel>` and updates the acceleration limit.
+// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_ACCEL`.
 bool parseAccel(const String& line) {
   int firstSpace = line.indexOf(' ');
   int secondSpace = line.indexOf(' ', firstSpace + 1);
@@ -392,6 +457,8 @@ bool parseAccel(const String& line) {
   return true;
 }
 
+// Parses `ENABLE <id> <0|1>` and toggles the selected driver.
+// Replies: `OK`, `ERR BAD_ID`.
 bool parseEnable(const String& line) {
   int id, en;
   if (sscanf(line.c_str(), "ENABLE %d %d", &id, &en) == 2) {
@@ -407,6 +474,8 @@ bool parseEnable(const String& line) {
   return false;
 }
 
+// Parses `ZERO <id>` and declares the current position to be home.
+// Replies: `OK`, `ERR BAD_ID`.
 bool parseZero(const String& line) {
   int id;
   if (sscanf(line.c_str(), "ZERO %d", &id) == 1) {
@@ -428,6 +497,8 @@ bool parseZero(const String& line) {
   return false;
 }
 
+// Parses `HOME <id>` or `HOMEALL` and kicks off the homing sequence.
+// Replies: `OK`, `ERR BAD_ID`. Successful completion later emits debug line `HOMED <id>`.
 bool parseHome(const String& line) {
   int id;
   if (sscanf(line.c_str(), "HOME %d", &id) == 1) {
@@ -450,6 +521,8 @@ bool parseHome(const String& line) {
   return false;
 }
 
+// Parses `SWEEP <id> <accel> <speed>` and enables or disables end-to-end motion.
+// Replies: `OK`, `ERR BAD_ID`.
 bool parseSweep(const String& line) {
   int firstSpace = line.indexOf(' ');
   int secondSpace = line.indexOf(' ', firstSpace + 1);
@@ -486,6 +559,9 @@ bool parseSweep(const String& line) {
   return true;
 }
 
+// Answers `POS?` with current motion state for every gauge.
+// Emits one `POS <id> <cur> <tgt> <homed> <homingState> <sweep>` line per gauge,
+// then replies `OK`.
 bool parsePosQuery(const String& line) {
   if (line == "POS?") {
     for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
@@ -508,6 +584,8 @@ bool parsePosQuery(const String& line) {
   return false;
 }
 
+// Answers the mandatory life question: are you there?
+// Reply: `PONG`.
 bool parsePing(const String& line) {
   if (line == "PING") {
     sendReply("PONG");
@@ -516,6 +594,8 @@ bool parsePing(const String& line) {
   return false;
 }
 
+// Answers `LED?` with the current RGB values for every configured LED.
+// Emits one `LED <id> <idx> <r> <g> <b>` line per configured LED, then replies `OK`.
 bool parseLedQuery(const String& line) {
   if (line == "LED?") {
     for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
@@ -539,6 +619,8 @@ bool parseLedQuery(const String& line) {
   return false;
 }
 
+// Parses `LED <id> <idx|a-b> <r> <g> <b>` and writes static colours.
+// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_IDX`.
 bool parseLed(const String& line) {
   int id, r, g, b;
   char idxToken[16];
@@ -562,12 +644,12 @@ bool parseLed(const String& line) {
   return false;
 }
 
+// Parses `BLINK ...` and assigns a simple on/off effect to one LED or a range.
+// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_IDX`, `ERR BAD_TIME`.
 bool parseBlink(const String& line) {
   int id, onMs, offMs, r, g, b;
   char idxToken[16];
-  // Accept both forms:
-  //   BLINK <id> <idx> <on_ms> <off_ms>           — use current LED colour
-  //   BLINK <id> <idx> <on_ms> <off_ms> <r> <g> <b> — set colour in same command
+  // Optional RGB values let BLINK either reuse or replace the current colour.
   int count = sscanf(line.c_str(), "BLINK %d %15s %d %d %d %d %d",
                      &id, idxToken, &onMs, &offMs, &r, &g, &b);
   if (count != 4 && count != 7) return false;
@@ -590,7 +672,7 @@ bool parseBlink(const String& line) {
 
   CRGB color = (count == 7)
     ? CRGB(constrain(r, 0, 255), constrain(g, 0, 255), constrain(b, 0, 255))
-    : CRGB(0, 0, 0);  // placeholder; overwritten per-LED below when count==4
+    : CRGB(0, 0, 0);  // Placeholder; replaced with the live LED colour below.
 
   unsigned long nowMs = millis();
   for (int i = idxFirst; i <= idxLast; i++) {
@@ -610,6 +692,8 @@ bool parseBlink(const String& line) {
   return true;
 }
 
+// Parses `BREATHE ...` and assigns a triangle-wave fade effect.
+// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_IDX`, `ERR BAD_TIME`.
 bool parseBreathe(const String& line) {
   int id, periodMs, r, g, b;
   char idxToken[16];
@@ -641,6 +725,8 @@ bool parseBreathe(const String& line) {
   return true;
 }
 
+// Parses `DFLASH ...` and assigns the double-flash pattern.
+// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_IDX`.
 bool parseDflash(const String& line) {
   int id, r, g, b;
   char idxToken[16];
@@ -670,6 +756,7 @@ bool parseDflash(const String& line) {
   return true;
 }
 
+// Advances all active LED effects and marks the strip dirty when something changed.
 void updateBlink() {
   unsigned long nowMs = millis();
   bool changed = false;
@@ -693,10 +780,11 @@ void updateBlink() {
         }
         case FX_BREATHE: {
           unsigned long dt = nowMs - bs.lastMs;
-          if (dt < 16) break;
+          if (dt < 64) break;
           uint32_t newPos = (uint32_t)bs.cyclePos + dt;
           bs.cyclePos = (uint16_t)(newPos % bs.periodMs);
           bs.lastMs = nowMs;
+          // Cheap triangle wave. It does the job and nobody has complained yet.
           uint16_t half = bs.periodMs >> 1;
           uint8_t bri = (bs.cyclePos < half)
               ? (uint8_t)((uint32_t)bs.cyclePos * 255 / half)
@@ -707,7 +795,7 @@ void updateBlink() {
           break;
         }
         case FX_DFLASH: {
-          static const uint16_t dur[4] = {100, 100, 100, 700};
+          static const uint16_t dur[4] = {100, 100, 100, 700}; // on, off, on, longer off
           if ((nowMs - bs.lastMs) >= dur[bs.dphase]) {
             bs.lastMs = nowMs;
             bs.dphase = (bs.dphase + 1) & 3;
@@ -723,6 +811,8 @@ void updateBlink() {
   if (changed) ledsDirty = true;
 }
 
+// Runs the command parsers in order until one claims the line.
+// Reply: `ERR BAD_CMD` when no parser accepts the line.
 void processLine(const String& line) {
   if (parseSet(line)) return;
   if (parseSpeed(line)) return;
@@ -742,6 +832,8 @@ void processLine(const String& line) {
   sendReply("ERR BAD_CMD");
 }
 
+// Reads newline-delimited commands from serial and hands complete lines to the parser.
+// Reply: `ERR TOO_LONG` when the buffered line exceeds the receive limit before newline.
 void readCommands() {
   while (CMD_PORT.available()) {
     char c = (char)CMD_PORT.read();
@@ -762,8 +854,10 @@ void readCommands() {
   }
 }
 
+// Initialises pins, LED bookkeeping and the initial homing cycle.
+// Reply/event: emits `READY` on CMD_PORT once boot is complete.
 void setup() {
-  DEBUG_PORT.begin(115200);
+  DEBUG_PORT.begin(SERIAL_BAUD);
   DEBUG_PORT.println("Gauge controller booting");
 
   for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
@@ -781,7 +875,7 @@ void setup() {
     gauges[i].lastUpdateMicros = micros();
   }
 
-  // Compute per-gauge LED offsets and initialise the strip.
+  // Flatten the per-gauge LED counts into offsets on the shared strip.
   uint8_t ledOff = 0;
   for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
     gaugeLedOffset[i] = ledOff;
@@ -794,19 +888,23 @@ void setup() {
   requestHomeAll();
 
   DEBUG_PORT.println("READY");
+  // Boot-complete handshake for the command channel.
   sendReply("READY");
 }
 
+// Main service loop: ingest commands, advance effects, move gauges, flush LEDs.
 void loop() {
   readCommands();
   updateBlink();
 
+  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
+    updateGauge(i);
+  }
+  
   if (ledsDirty) {
     FastLED.show();
     ledsDirty = false;
   }
 
-  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
-    updateGauge(i);
-  }
+
 }

config.example.json

@@ -33,6 +33,8 @@
       "min": 0,
       "max": 7300,
       "max_steps": 4000,
+      "speed": 5000,
+      "acceleration": 6000,
       "unit": "W",
       "leds": {
         "ws2812_red":   [255, 0, 0],
@@ -45,6 +47,8 @@
       "min": 0,
       "max": 7300,
       "max_steps": 4000,
+      "speed": 5000,
+      "acceleration": 6000,
       "unit": "W",
       "leds": {
         "ws2812_red":   [255, 0, 0],

gauge.py

@@ -24,6 +24,7 @@ Additional config.json fields:
 import network
 import utime
 import ujson
+import gc
 from umqtt.robust import MQTTClient
 from machine import UART
 
@@ -114,6 +115,8 @@ if "gauges" in _cfg:
                 "min": float(g.get("min", 0)),
                 "max": float(g.get("max", 100)),
                 "max_steps": int(g.get("max_steps", 4000)),
+                "speed": float(g.get("speed", 5000.0)),
+                "acceleration": float(g.get("acceleration", 6000.0)),
                 "entity_name": g.get("entity_name", f"Gauge {i + 1}"),
                 "unit": g.get("unit", ""),
                 "ws2812_red": tuple(led_cfg.get("ws2812_red", [255, 0, 0])),
@@ -128,6 +131,8 @@ else:
             "min": float(_cfg.get("gauge_min", 0)),
             "max": float(_cfg.get("gauge_max", 7300)),
             "max_steps": int(_cfg.get("gauge_max_steps", 4000)),
+            "speed": float(_cfg.get("gauge_speed", 5000.0)),
+            "acceleration": float(_cfg.get("gauge_acceleration", 6000.0)),
             "entity_name": _cfg.get("gauge_entity_name", "Selsyn 1 Power"),
             "unit": _cfg.get("gauge_unit", "W"),
             "ws2812_red": tuple(_cfg.get("ws2812_red", [255, 0, 0])),
@@ -194,6 +199,14 @@ def gauge_zero(gauge_idx):
     arduino_send(f"ZERO {gauge_idx}")
 
 
+def gauge_set_speed(gauge_idx, speed):
+    arduino_send(f"SPEED {gauge_idx} {speed}")
+
+
+def gauge_set_acceleration(gauge_idx, acceleration):
+    arduino_send(f"ACCEL {gauge_idx} {acceleration}")
+
+
 def _set_led(gauge_idx, idx, r, g, b):
     arduino_send(f"LED {gauge_idx} {idx} {r} {g} {b}")
 
@@ -251,6 +264,8 @@ def _apply_blink_or_led(gauge_idx, led_idx, color, effect):
 
 gauge_targets = [g["min"] for g in gauges]
 gauge_last_rezero = [utime.ticks_ms() for _ in gauges]
+gauge_speeds = [g["speed"] for g in gauges]
+gauge_accelerations = [g["acceleration"] for g in gauges]
 
 backlight_color = [(0, 0, 0) for _ in range(num_gauges)]
 backlight_brightness = [100 for _ in range(num_gauges)]
@@ -378,6 +393,12 @@ def make_gauge_topics(prefix, gauge_id):
         "status": f"{prefix}/gauge{gauge_id}/status",
         "zero": f"{prefix}/gauge{gauge_id}/zero",
         "disc": f"homeassistant/number/{MQTT_CLIENT_ID}_g{gauge_id}/config",
+        "speed": f"{prefix}/gauge{gauge_id}/speed/set",
+        "speed_state": f"{prefix}/gauge{gauge_id}/speed/state",
+        "speed_disc": f"homeassistant/number/{MQTT_CLIENT_ID}_g{gauge_id}_speed/config",
+        "acceleration": f"{prefix}/gauge{gauge_id}/acceleration/set",
+        "acceleration_state": f"{prefix}/gauge{gauge_id}/acceleration/state",
+        "acceleration_disc": f"homeassistant/number/{MQTT_CLIENT_ID}_g{gauge_id}_acceleration/config",
         "led_red": f"{prefix}/gauge{gauge_id}/led/red/set",
         "led_green": f"{prefix}/gauge{gauge_id}/led/green/set",
         "led_bl": f"{prefix}/gauge{gauge_id}/led/backlight/set",
@@ -503,6 +524,32 @@ def on_message(topic, payload):
                 warn(f"Invalid set value for gauge {i}: '{payload}'")
             return
 
+        if topic == gt["speed"]:
+            try:
+                speed = float(payload)
+                if speed <= 0.0:
+                    raise ValueError
+                gauge_speeds[i] = speed
+                gauge_set_speed(i, speed)
+                _publish(gt["speed_state"], str(speed), retain=True)
+                info(f"Gauge {i} speed -> {speed}")
+            except ValueError:
+                warn(f"Invalid speed for gauge {i}: '{payload}'")
+            return
+
+        if topic == gt["acceleration"]:
+            try:
+                acceleration = float(payload)
+                if acceleration <= 0.0:
+                    raise ValueError
+                gauge_accelerations[i] = acceleration
+                gauge_set_acceleration(i, acceleration)
+                _publish(gt["acceleration_state"], str(acceleration), retain=True)
+                info(f"Gauge {i} acceleration -> {acceleration}")
+            except ValueError:
+                warn(f"Invalid acceleration for gauge {i}: '{payload}'")
+            return
+
         if topic == gt["led_red"]:
             try:
                 data = ujson.loads(payload)
@@ -680,6 +727,8 @@ def _subscribe_all(c):
         prefix = f"{MQTT_PREFIX}/gauge{i}"
         c.subscribe(f"{prefix}/set")
         c.subscribe(f"{prefix}/zero")
+        c.subscribe(f"{prefix}/speed/set")
+        c.subscribe(f"{prefix}/acceleration/set")
         c.subscribe(f"{prefix}/led/red/set")
         c.subscribe(f"{prefix}/led/green/set")
         c.subscribe(f"{prefix}/led/backlight/set")
@@ -707,6 +756,10 @@ def connect_mqtt():
 
 _mqtt_check_interval_ms = 30000
 _last_mqtt_check = 0
+_discovery_phases = ()
+_discovery_phase_idx = 0
+_last_discovery_ms = 0
+_DISCOVERY_INTERVAL_MS = 500
 
 
 def check_mqtt():
@@ -742,7 +795,7 @@ def check_mqtt():
             client_ref.connect()
             _mqtt_connected = True
             info("MQTT reconnected!")
-            publish_discovery(client_ref)
+            schedule_discovery()
             _subscribe_all(client_ref)
             publish_online(client_ref)
             publish_state(client_ref)
@@ -756,6 +809,199 @@ def check_mqtt():
     return False
 
 
+def _discovery_pause(client, count=5, delay_ms=25):
+    for _ in range(count):
+        client.check_msg()
+        utime.sleep_ms(delay_ms)
+    gc.collect()
+
+
+def _publish_discovery_entity(client, topic, payload, log_msg):
+    client.publish(topic, ujson.dumps(payload), retain=True)
+    info(log_msg)
+
+
+def _publish_gauge_discovery(client, dev_ref):
+    for i, g in enumerate(gauges):
+        gt = gauge_topics[i]
+        _publish_discovery_entity(
+            client,
+            gt["disc"],
+            {
+                "name": g["entity_name"],
+                "unique_id": f"{MQTT_CLIENT_ID}_g{i}",
+                "cmd_t": gt["set"],
+                "stat_t": gt["state"],
+                "avty_t": gt["status"],
+                "min": g["min"],
+                "max": g["max"],
+                "step": 1,
+                "unit_of_meas": g["unit"],
+                "icon": "mdi:gauge",
+                "dev": dev_ref,
+            },
+            f"Discovery: gauge {i} ({g['name']})",
+        )
+    _discovery_pause(client)
+
+
+def _publish_speed_discovery(client, dev_ref):
+    for i, g in enumerate(gauges):
+        gt = gauge_topics[i]
+        _publish_discovery_entity(
+            client,
+            gt["speed_disc"],
+            {
+                "name": f"{g['name']} Speed",
+                "unique_id": f"{MQTT_CLIENT_ID}_g{i}_speed",
+                "cmd_t": gt["speed"],
+                "stat_t": gt["speed_state"],
+                "avty_t": gt["status"],
+                "min": 1,
+                "max": 50000,
+                "step": 1,
+                "mode": "box",
+                "unit_of_meas": "steps/s",
+                "icon": "mdi:speedometer",
+                "entity_category": "config",
+                "dev": dev_ref,
+            },
+            f"Discovery: gauge {i} speed",
+        )
+    _discovery_pause(client)
+
+
+def _publish_acceleration_discovery(client, dev_ref):
+    for i, g in enumerate(gauges):
+        gt = gauge_topics[i]
+        _publish_discovery_entity(
+            client,
+            gt["acceleration_disc"],
+            {
+                "name": f"{g['name']} Acceleration",
+                "unique_id": f"{MQTT_CLIENT_ID}_g{i}_acceleration",
+                "cmd_t": gt["acceleration"],
+                "stat_t": gt["acceleration_state"],
+                "avty_t": gt["status"],
+                "min": 1,
+                "max": 100000,
+                "step": 1,
+                "mode": "box",
+                "unit_of_meas": "steps/s2",
+                "icon": "mdi:chart-bell-curve-cumulative",
+                "entity_category": "config",
+                "dev": dev_ref,
+            },
+            f"Discovery: gauge {i} acceleration",
+        )
+    _discovery_pause(client)
+
+
+def _publish_indicator_led_discovery(client, dev_ref):
+    for i, g in enumerate(gauges):
+        gt = gauge_topics[i]
+        _publish_discovery_entity(
+            client,
+            gt["led_red_disc"],
+            {
+                "name": f"{g['name']} Dial Red LED",
+                "uniq_id": f"{MQTT_CLIENT_ID}_g{i}_red",
+                "cmd_t": gt["led_red"],
+                "stat_t": gt["led_red_state"],
+                "schema": "json",
+                "supported_color_modes": ["onoff"],
+                "effect": True,
+                "effect_list": _EFFECT_LIST,
+                "icon": "mdi:led-on",
+                "dev": dev_ref,
+                "ret": True,
+            },
+            f"Discovery: gauge {i} red LED",
+        )
+
+        _publish_discovery_entity(
+            client,
+            gt["led_green_disc"],
+            {
+                "name": f"{g['name']} Dial Green LED",
+                "uniq_id": f"{MQTT_CLIENT_ID}_g{i}_green",
+                "cmd_t": gt["led_green"],
+                "stat_t": gt["led_green_state"],
+                "schema": "json",
+                "supported_color_modes": ["onoff"],
+                "effect": True,
+                "effect_list": _EFFECT_LIST,
+                "icon": "mdi:led-on",
+                "dev": dev_ref,
+                "ret": True,
+            },
+            f"Discovery: gauge {i} green LED",
+        )
+    _discovery_pause(client)
+
+
+def _publish_backlight_status_discovery(client, dev_ref):
+    for i, g in enumerate(gauges):
+        gt = gauge_topics[i]
+        _publish_discovery_entity(
+            client,
+            gt["led_bl_disc"],
+            {
+                "name": f"{g['name']} Backlight",
+                "uniq_id": f"{MQTT_CLIENT_ID}_g{i}_bl",
+                "cmd_t": gt["led_bl"],
+                "stat_t": gt["led_bl_state"],
+                "schema": "json",
+                "supported_color_modes": ["rgb"],
+                "effect": True,
+                "effect_list": _EFFECT_LIST,
+                "icon": "mdi:led-strip",
+                "dev": dev_ref,
+                "ret": True,
+            },
+            f"Discovery: gauge {i} backlight",
+        )
+
+        _publish_discovery_entity(
+            client,
+            gt["status_red_disc"],
+            {
+                "name": f"{g['name']} Channel Status Red",
+                "uniq_id": f"{MQTT_CLIENT_ID}_g{i}_status_red",
+                "cmd_t": gt["status_red"],
+                "stat_t": gt["status_red_state"],
+                "schema": "json",
+                "supported_color_modes": ["onoff"],
+                "effect": True,
+                "effect_list": _EFFECT_LIST,
+                "icon": "mdi:led-on",
+                "dev": dev_ref,
+                "ret": True,
+            },
+            f"Discovery: gauge {i} status red",
+        )
+
+        _publish_discovery_entity(
+            client,
+            gt["status_green_disc"],
+            {
+                "name": f"{g['name']} Channel Status Green",
+                "uniq_id": f"{MQTT_CLIENT_ID}_g{i}_status_green",
+                "cmd_t": gt["status_green"],
+                "stat_t": gt["status_green_state"],
+                "schema": "json",
+                "supported_color_modes": ["onoff"],
+                "effect": True,
+                "effect_list": _EFFECT_LIST,
+                "icon": "mdi:led-on",
+                "dev": dev_ref,
+                "ret": True,
+            },
+            f"Discovery: gauge {i} status green",
+        )
+    _discovery_pause(client)
+
+
 def publish_discovery(client):
     """Publish all HA MQTT discovery payloads for gauges and LEDs."""
     _dev_ref = _DEVICE
@@ -769,139 +1015,54 @@ def publish_discovery(client):
             f"homeassistant/switch/{MQTT_CLIENT_ID}_g{i}_status_green/config",
         ]:
             client.publish(old_t, b"", retain=True)
+        _discovery_pause(client, count=2, delay_ms=15)
 
-    for i, g in enumerate(gauges):
-        gt = gauge_topics[i]
+    _publish_gauge_discovery(client, _dev_ref)
+    _publish_speed_discovery(client, _dev_ref)
+    _publish_acceleration_discovery(client, _dev_ref)
+    _publish_indicator_led_discovery(client, _dev_ref)
+    _publish_backlight_status_discovery(client, _dev_ref)
 
-        client.publish(
-            gt["disc"],
-            ujson.dumps(
-                {
-                    "name": g["entity_name"],
-                    "unique_id": f"{MQTT_CLIENT_ID}_g{i}",
-                    "cmd_t": gt["set"],
-                    "stat_t": gt["state"],
-                    "avty_t": gt["status"],
-                    "min": g["min"],
-                    "max": g["max"],
-                    "step": 1,
-                    "unit_of_meas": g["unit"],
-                    "icon": "mdi:gauge",
-                    "dev": _dev_ref,
-                }
-            ),
-            retain=True,
-        )
-        info(f"Discovery: gauge {i} ({g['name']})")
 
-        for _ in range(5):
-            client.check_msg()
-            utime.sleep_ms(10)
+def schedule_discovery():
+    global _discovery_phases, _discovery_phase_idx, _last_discovery_ms
+    _dev_ref = _DEVICE
+    _discovery_phases = (
+        lambda client: _clear_legacy_discovery(client),
+        lambda client: _publish_gauge_discovery(client, _dev_ref),
+        lambda client: _publish_speed_discovery(client, _dev_ref),
+        lambda client: _publish_acceleration_discovery(client, _dev_ref),
+        lambda client: _publish_indicator_led_discovery(client, _dev_ref),
+        lambda client: _publish_backlight_status_discovery(client, _dev_ref),
+    )
+    _discovery_phase_idx = 0
+    _last_discovery_ms = 0
 
-        client.publish(
-            gt["led_red_disc"],
-            ujson.dumps({
-                "name": f"{g['name']} Red LED",
-                "uniq_id": f"{MQTT_CLIENT_ID}_g{i}_red",
-                "cmd_t": gt["led_red"],
-                "stat_t": gt["led_red_state"],
-                "schema": "json",
-                "supported_color_modes": ["onoff"],
-                "effect": True,
-                "effect_list": _EFFECT_LIST,
-                "icon": "mdi:led-on",
-                "dev": _dev_ref,
-                "ret": True,
-            }),
-            retain=True,
-        )
-        info(f"Discovery: gauge {i} red LED")
 
-        client.publish(
-            gt["led_green_disc"],
-            ujson.dumps({
-                "name": f"{g['name']} Green LED",
-                "uniq_id": f"{MQTT_CLIENT_ID}_g{i}_green",
-                "cmd_t": gt["led_green"],
-                "stat_t": gt["led_green_state"],
-                "schema": "json",
-                "supported_color_modes": ["onoff"],
-                "effect": True,
-                "effect_list": _EFFECT_LIST,
-                "icon": "mdi:led-on",
-                "dev": _dev_ref,
-                "ret": True,
-            }),
-            retain=True,
-        )
-        info(f"Discovery: gauge {i} green LED")
+def _clear_legacy_discovery(client):
+    for i in range(num_gauges):
+        for old_t in [
+            f"homeassistant/switch/{MQTT_CLIENT_ID}_g{i}_red/config",
+            f"homeassistant/switch/{MQTT_CLIENT_ID}_g{i}_green/config",
+            f"homeassistant/switch/{MQTT_CLIENT_ID}_g{i}_status_red/config",
+            f"homeassistant/switch/{MQTT_CLIENT_ID}_g{i}_status_green/config",
+        ]:
+            client.publish(old_t, b"", retain=True)
+        _discovery_pause(client, count=2, delay_ms=15)
 
-        for _ in range(5):
-            client.check_msg()
-            utime.sleep_ms(10)
 
-        client.publish(
-            gt["led_bl_disc"],
-            ujson.dumps(
-                {
-                    "name": f"{g['name']} Backlight",
-                    "uniq_id": f"{MQTT_CLIENT_ID}_g{i}_bl",
-                    "cmd_t": gt["led_bl"],
-                    "stat_t": gt["led_bl_state"],
-                    "schema": "json",
-                    "supported_color_modes": ["rgb"],
-                    "effect": True,
-                    "effect_list": _EFFECT_LIST,
-                    "icon": "mdi:led-strip",
-                    "dev": _dev_ref,
-                    "ret": True,
-                }
-            ),
-            retain=True,
-        )
-        info(f"Discovery: gauge {i} backlight")
+def service_discovery():
+    global _discovery_phase_idx, _last_discovery_ms
+    if client_ref is None or _discovery_phase_idx >= len(_discovery_phases):
+        return
 
-        client.publish(
-            gt["status_red_disc"],
-            ujson.dumps({
-                "name": f"{g['name']} Status Red",
-                "uniq_id": f"{MQTT_CLIENT_ID}_g{i}_status_red",
-                "cmd_t": gt["status_red"],
-                "stat_t": gt["status_red_state"],
-                "schema": "json",
-                "supported_color_modes": ["onoff"],
-                "effect": True,
-                "effect_list": _EFFECT_LIST,
-                "icon": "mdi:led-on",
-                "dev": _dev_ref,
-                "ret": True,
-            }),
-            retain=True,
-        )
-        info(f"Discovery: gauge {i} status red")
+    now = utime.ticks_ms()
+    if _last_discovery_ms and utime.ticks_diff(now, _last_discovery_ms) < _DISCOVERY_INTERVAL_MS:
+        return
 
-        client.publish(
-            gt["status_green_disc"],
-            ujson.dumps({
-                "name": f"{g['name']} Status Green",
-                "uniq_id": f"{MQTT_CLIENT_ID}_g{i}_status_green",
-                "cmd_t": gt["status_green"],
-                "stat_t": gt["status_green_state"],
-                "schema": "json",
-                "supported_color_modes": ["onoff"],
-                "effect": True,
-                "effect_list": _EFFECT_LIST,
-                "icon": "mdi:led-on",
-                "dev": _dev_ref,
-                "ret": True,
-            }),
-            retain=True,
-        )
-        info(f"Discovery: gauge {i} status green")
-
-        for _ in range(5):
-            client.check_msg()
-            utime.sleep_ms(10)
+    _discovery_phases[_discovery_phase_idx](client_ref)
+    _discovery_phase_idx += 1
+    _last_discovery_ms = utime.ticks_ms()
 
 
 def publish_online(client):
@@ -913,6 +1074,14 @@ def publish_state(client):
     for i in range(num_gauges):
         gt = gauge_topics[i]
         client.publish(gt["state"], str(gauge_targets[i]))
+        client.publish(gt["speed_state"], str(gauge_speeds[i]), retain=True)
+        client.publish(gt["acceleration_state"], str(gauge_accelerations[i]), retain=True)
+
+
+def apply_motion_defaults():
+    for i in range(num_gauges):
+        gauge_set_speed(i, gauge_speeds[i])
+        gauge_set_acceleration(i, gauge_accelerations[i])
 
 
 # ---------------------------------------------------------------------------
@@ -928,11 +1097,10 @@ def main():
     connect_wifi(WIFI_SSID, WIFI_PASSWORD)
 
     connect_mqtt()
-
-    info("Publishing discovery...")
-    publish_discovery(client_ref)
-
     _subscribe_all(client_ref)
+    schedule_discovery()
+
+    apply_motion_defaults()
     info("Draining initial retained messages...")
     for _ in range(50):
         client_ref.check_msg()
@@ -960,6 +1128,7 @@ def main():
         ota.mark_ok()
     except:
         pass
+    gc.collect()
 
     last_heartbeat = utime.ticks_ms()
 
@@ -968,14 +1137,17 @@ def main():
             now = utime.ticks_ms()
 
             check_wifi()
+            gc.collect()
 
             if not check_mqtt():
                 utime.sleep_ms(1000)
                 continue
 
             client_ref.check_msg()
+            service_discovery()
             arduino_recv()
             _flush_backlight_state()
+            gc.collect()
 
             # Periodic re-home
             for i in range(num_gauges):
@@ -988,6 +1160,8 @@ def main():
                 info(f"Heartbeat: {gauge_targets}")
                 publish_state(client_ref)
                 last_heartbeat = now
+                gc.collect()
+                info(f"Heap free: {gc.mem_free()} bytes")
 
             utime.sleep_ms(10)
 
@@ -995,6 +1169,7 @@ def main():
             import sys
             sys.print_exception(e)
             log_err(f"Main loop error: {e} — continuing")
+            gc.collect()
             utime.sleep_ms(100)