Refactor mcu code. Closes #11

gemini.md

@@ -110,7 +110,11 @@ esp32c3-wiji/
 ├── platformio.ini                # PlatformIO config (env: esp32-c3-devkitm-1)
 ├── fivebarIKGame.js              # REFERENCE ONLY — original site's kinematics (do not import)
 ├── src/
-│   └── main.cpp                  # ESP32 firmware (BLE + AccelStepper)
+│   ├── Config.h                  # Pins, Constants, UUIDs
+│   ├── BleManager.cpp            # BLE setup, callbacks, notifications
+│   ├── MotorController.cpp       # AccelStepper wrap, homing state, idle timeout
+│   ├── StatusLed.cpp             # Onboard LED feedback logic
+│   └── main.cpp                  # Main app glue, command router
 └── web/
     ├── styles/
     │   ├── base.css              # CSS variables, reset, typography, layout primitives
@@ -387,7 +391,7 @@ The `?v=Date.now()` on imports handles this automatically on page load.
   - JSON export to persist sequences to `sequences.json`.
   - Quick Sequences UI in `stepper-test`, `board-control`, and `input-text`.
   - Shared `sequence-runner.js` to handle asynchronous execution and delays.
-- [x] Firmware (`src/main.cpp`) — AccelStepper + BLE command parser + position NOTIFY
+- [x] Firmware (Modularized) — `BleManager`, `MotorController`, `StatusLed`, and `main.cpp` for routing.
 
 ---
 

src/BleManager.cpp

@@ -0,0 +1,84 @@
+#include "BleManager.h"
+#include "Config.h"
+
+BleManager* g_bleManagerInstance = nullptr;
+
+class ServerCallbacks : public BLEServerCallbacks {
+  void onConnect(BLEServer *) override {
+    if (g_bleManagerInstance) g_bleManagerInstance->setConnected(true);
+    Serial.println("[BLE] Client connected");
+  }
+  void onDisconnect(BLEServer *) override {
+    if (g_bleManagerInstance) g_bleManagerInstance->setConnected(false);
+    Serial.println("[BLE] Client disconnected – restarting advertising");
+    BLEDevice::startAdvertising();
+  }
+};
+
+class CmdCallbacks : public BLECharacteristicCallbacks {
+  void onWrite(BLECharacteristic *pChar) override {
+    String val = pChar->getValue().c_str();
+    val.trim();
+    if (val.length() == 0) return;
+    Serial.printf("[CMD] Received: '%s'\n", val.c_str());
+    if (g_bleManagerInstance) g_bleManagerInstance->handleCommand(val);
+  }
+};
+
+BleManager::BleManager() {
+    g_bleManagerInstance = this;
+}
+
+void BleManager::init(std::function<void(const String&)> commandCallback) {
+    onCommand = commandCallback;
+    
+    BLEDevice::init(DEVICE_NAME);
+    BLEServer *pServer = BLEDevice::createServer();
+    pServer->setCallbacks(new ServerCallbacks());
+
+    BLEService *pService = pServer->createService(SERVICE_UUID);
+
+    BLECharacteristic* pCmdChar = pService->createCharacteristic(CMD_UUID, BLECharacteristic::PROPERTY_WRITE);
+    pCmdChar->setCallbacks(new CmdCallbacks());
+
+    pStatusChar = pService->createCharacteristic(STATUS_UUID, BLECharacteristic::PROPERTY_NOTIFY);
+
+    pService->start();
+
+    BLEAdvertising *pAdv = BLEDevice::getAdvertising();
+    pAdv->addServiceUUID(SERVICE_UUID);
+    pAdv->setScanResponse(true);
+    pAdv->setMinPreferred(0x06); // 7.5ms
+    pAdv->setMaxPreferred(0x0C); // 15ms
+    BLEDevice::startAdvertising();
+
+    Serial.printf("[BLE] Advertising as '%s' – ready!\n", DEVICE_NAME);
+}
+
+void BleManager::setConnected(bool state) {
+    connected = state;
+}
+
+bool BleManager::isConnected() const {
+    return connected;
+}
+
+void BleManager::handleCommand(const String& cmd) {
+    if (onCommand) {
+        onCommand(cmd);
+    }
+}
+
+void BleManager::notifyPosition(const String& pos) {
+    if (!connected || !pStatusChar) return;
+    pStatusChar->setValue(pos.c_str());
+    pStatusChar->notify();
+    Serial.printf("[POS] %s\n", pos.c_str());
+}
+
+void BleManager::notifyMessage(const String& msg) {
+    if (!connected || !pStatusChar) return;
+    pStatusChar->setValue(msg.c_str());
+    pStatusChar->notify();
+    Serial.printf("[NOTIFY] %s\n", msg.c_str());
+}

src/BleManager.h

@@ -0,0 +1,23 @@
+#pragma once
+#include <Arduino.h>
+#include <functional>
+#include <BLEDevice.h>
+#include <BLEServer.h>
+
+class BleManager {
+public:
+    BleManager();
+    void init(std::function<void(const String&)> commandCallback);
+    void notifyPosition(const String& pos);
+    void notifyMessage(const String& msg);
+    bool isConnected() const;
+
+    // These need to be public for callbacks
+    void setConnected(bool state);
+    void handleCommand(const String& cmd);
+
+private:
+    std::function<void(const String&)> onCommand;
+    bool connected = false;
+    BLECharacteristic *pStatusChar = nullptr;
+};

src/Config.h

@@ -0,0 +1,40 @@
+#pragma once
+#include <Arduino.h>
+
+// ─── Motor pin mapping (28BYJ-48 / ULN2003) ──────────────────────
+// Motor 1 – Shoulder
+#define M1_IN1 0
+#define M1_IN2 1
+#define M1_IN3 3
+#define M1_IN4 4
+
+// Motor 2 – Elbow
+#define M2_IN1 5
+#define M2_IN2 6
+#define M2_IN3 7
+#define M2_IN4 10
+
+// ─── Stepper constants ────────────────────────────────────────────
+#define STEPS_PER_REV 2048
+#define DEFAULT_SPEED 600.0f
+#define DEFAULT_ACCEL 100.0f
+#define IDLE_TIMEOUT_MS (30 * 60 * 1000)
+#define NOTIFY_INTERVAL_MS 200
+
+// ─── BLE Constants ────────────────────────────────────────────────
+#define DEVICE_NAME "WijiBoard"
+#define SERVICE_UUID "18f3b235-9831-4c75-8ec0-210469b820a0"
+#define CMD_UUID "cd083b06-4447-4cf3-a7c3-322ecf802ce4"
+#define STATUS_UUID "82e38c5b-d3ab-41d1-861c-b84dc6bb1e03"
+
+// ─── LED Definitions ──────────────────────────────────────────────
+#ifndef STATUS_LED_PIN
+  #ifdef LED_BUILTIN
+    #define STATUS_LED_PIN LED_BUILTIN
+  #endif
+#endif
+
+#ifndef STATUS_LED_ON
+  #define STATUS_LED_ON HIGH
+  #define STATUS_LED_OFF LOW
+#endif

src/MotorController.cpp

@@ -0,0 +1,180 @@
+#include "MotorController.h"
+#include "Config.h"
+
+MotorController::MotorController() 
+    : stepper1(AccelStepper::FULL4WIRE, M1_IN1, M1_IN2, M1_IN3, M1_IN4),
+      stepper2(AccelStepper::FULL4WIRE, M2_IN1, M2_IN2, M2_IN3, M2_IN4),
+      currentMaxSpeed(DEFAULT_SPEED),
+      currentAccel(DEFAULT_ACCEL),
+      lastMotorActive(0),
+      motorsDisabled(true),
+      homingState(HOME_IDLE) {}
+
+void MotorController::init() {
+    stepper1.setMaxSpeed(DEFAULT_SPEED);
+    stepper1.setAcceleration(DEFAULT_ACCEL);
+    stepper1.setCurrentPosition(0);
+
+    stepper2.setMaxSpeed(DEFAULT_SPEED);
+    stepper2.setAcceleration(DEFAULT_ACCEL);
+    stepper2.setCurrentPosition(0);
+
+    Serial.println("[STEP] Steppers initialized");
+}
+
+void MotorController::setOnTimeoutCallback(std::function<void()> cb) {
+    onTimeout = cb;
+}
+
+void MotorController::update() {
+    stepper1.run();
+    stepper2.run();
+
+    bool moving = isMoving();
+    if (moving) {
+        lastMotorActive = millis();
+        motorsDisabled = false;
+    }
+
+    checkIdleTimeout();
+    processHoming();
+}
+
+bool MotorController::isMoving() {
+    return stepper1.isRunning() || stepper2.isRunning() || homingState != HOME_IDLE;
+}
+
+String MotorController::getPositionString() {
+    bool isHolding = !motorsDisabled || isMoving();
+    return "P:" + String(stepper1.currentPosition()) + "," +
+                  String(stepper2.currentPosition()) + "," +
+                  String(isHolding ? 1 : 0);
+}
+
+void MotorController::checkIdleTimeout() {
+    if (!motorsDisabled && !isMoving() && (millis() - lastMotorActive > IDLE_TIMEOUT_MS)) {
+        stepper1.disableOutputs();
+        stepper2.disableOutputs();
+        motorsDisabled = true;
+        Serial.println("[SYS] Idle timeout reached. Steppers disabled.");
+        if (onTimeout) {
+            onTimeout();
+        }
+    }
+}
+
+void MotorController::moveRelative1(long steps) {
+    stepper1.move(steps);
+    Serial.printf("[S1] Move relative %+ld steps\n", steps);
+}
+
+void MotorController::moveRelative2(long steps) {
+    stepper2.move(steps);
+    Serial.printf("[S2] Move relative %+ld steps\n", steps);
+}
+
+void MotorController::moveAbsolute(long target1, long target2) {
+    stepper1.moveTo(target1);
+    stepper2.moveTo(target2);
+    Serial.printf("[X] Move absolute to %ld, %ld\n", target1, target2);
+}
+
+void MotorController::setMaxSpeed(float spd) {
+    currentMaxSpeed = spd;
+    stepper1.setMaxSpeed(spd);
+    stepper2.setMaxSpeed(spd);
+    Serial.printf("[CFG] Max speed → %.0f steps/sec\n", spd);
+}
+
+void MotorController::setAcceleration(float acc) {
+    currentAccel = acc;
+    stepper1.setAcceleration(acc);
+    stepper2.setAcceleration(acc);
+    Serial.printf("[CFG] Acceleration → %.0f steps/sec²\n", acc);
+}
+
+void MotorController::homeAll() {
+    Serial.println("[SYS] Homing all: Phase 1 (Stall OUTWARD)");
+    stepper1.setMaxSpeed(1500);
+    stepper2.setMaxSpeed(1500);
+    stepper1.setAcceleration(1500);
+    stepper2.setAcceleration(1500);
+    stepper1.move(-2000); 
+    stepper2.move(2000);
+    homingState = HOME_ALL_PHASE1;
+}
+
+void MotorController::home1() {
+    Serial.println("[S1] Homing Motor 1: Phase 1 (Stall OUTWARD)");
+    stepper1.setMaxSpeed(1500);
+    stepper1.setAcceleration(1500);
+    stepper1.move(-2000);
+    homingState = HOME_1_PHASE1;
+}
+
+void MotorController::home2() {
+    Serial.println("[S2] Homing Motor 2: Phase 1 (Stall OUTWARD)");
+    stepper2.setMaxSpeed(1500);
+    stepper2.setAcceleration(1500);
+    stepper2.move(2000);
+    homingState = HOME_2_PHASE1;
+}
+
+void MotorController::zeroAll() {
+    stepper1.setCurrentPosition(1024);
+    stepper2.setCurrentPosition(0);
+    Serial.println("[SYS] Position blindly reset to home (1024, 0).");
+}
+
+void MotorController::disableMotors() {
+    stepper1.disableOutputs();
+    stepper2.disableOutputs();
+    motorsDisabled = true;
+    Serial.println("[SYS] Steppers manually disabled.");
+}
+
+void MotorController::processHoming() {
+    if (homingState == HOME_ALL_PHASE1) {
+        if (stepper1.distanceToGo() == 0 && stepper2.distanceToGo() == 0) {
+            stepper1.setCurrentPosition(1024);
+            stepper2.setCurrentPosition(0);
+            stepper1.moveTo(1024);
+            stepper2.moveTo(0);
+            homingState = HOME_ALL_PHASE2;
+            Serial.println("[SYS] Homing all: Phase 2 (Hold Outward Home)");
+        }
+    } else if (homingState == HOME_ALL_PHASE2) {
+        if (stepper1.distanceToGo() == 0 && stepper2.distanceToGo() == 0) {
+            stepper1.setMaxSpeed(currentMaxSpeed);
+            stepper2.setMaxSpeed(currentMaxSpeed);
+            stepper1.setAcceleration(currentAccel);
+            stepper2.setAcceleration(currentAccel);
+            homingState = HOME_IDLE;
+            Serial.println("[SYS] Homing all complete.");
+        }
+    } else if (homingState == HOME_1_PHASE1) {
+        if (stepper1.distanceToGo() == 0) {
+            stepper1.setCurrentPosition(1024);
+            stepper1.moveTo(1024);
+            homingState = HOME_1_PHASE2;
+        }
+    } else if (homingState == HOME_1_PHASE2) {
+        if (stepper1.distanceToGo() == 0) {
+            stepper1.setMaxSpeed(currentMaxSpeed);
+            stepper1.setAcceleration(currentAccel);
+            homingState = HOME_IDLE;
+        }
+    } else if (homingState == HOME_2_PHASE1) {
+        if (stepper2.distanceToGo() == 0) {
+            stepper2.setCurrentPosition(0);
+            stepper2.moveTo(0);
+            homingState = HOME_2_PHASE2;
+        }
+    } else if (homingState == HOME_2_PHASE2) {
+        if (stepper2.distanceToGo() == 0) {
+            stepper2.setMaxSpeed(currentMaxSpeed);
+            stepper2.setAcceleration(currentAccel);
+            homingState = HOME_IDLE;
+        }
+    }
+}

src/MotorController.h

@@ -0,0 +1,54 @@
+#pragma once
+#include <Arduino.h>
+#include <AccelStepper.h>
+#include <functional>
+
+class MotorController {
+public:
+    MotorController();
+    void init();
+    void update();
+
+    void moveRelative1(long steps);
+    void moveRelative2(long steps);
+    void moveAbsolute(long target1, long target2);
+    
+    void setMaxSpeed(float spd);
+    void setAcceleration(float acc);
+    
+    void home1();
+    void home2();
+    void homeAll();
+    void zeroAll();
+    void disableMotors();
+
+    void setOnTimeoutCallback(std::function<void()> cb);
+
+    bool isMoving();
+    String getPositionString();
+    
+private:
+    AccelStepper stepper1;
+    AccelStepper stepper2;
+
+    float currentMaxSpeed;
+    float currentAccel;
+    
+    unsigned long lastMotorActive;
+    bool motorsDisabled;
+    std::function<void()> onTimeout;
+    
+    enum HomingState {
+      HOME_IDLE,
+      HOME_ALL_PHASE1,
+      HOME_ALL_PHASE2,
+      HOME_1_PHASE1,
+      HOME_1_PHASE2,
+      HOME_2_PHASE1,
+      HOME_2_PHASE2
+    };
+    HomingState homingState;
+    
+    void processHoming();
+    void checkIdleTimeout();
+};

src/StatusLed.cpp

@@ -0,0 +1,32 @@
+#include "StatusLed.h"
+#include "Config.h"
+
+void StatusLed::init() {
+#ifdef STATUS_LED_PIN
+    pinMode(STATUS_LED_PIN, OUTPUT);
+    digitalWrite(STATUS_LED_PIN, STATUS_LED_OFF);
+#endif
+}
+
+void StatusLed::update(bool isConnected, bool isMoving) {
+#ifdef STATUS_LED_PIN
+    if (isMoving) {
+        unsigned long now = millis();
+        if (now - lastBlinkTime >= 150) {
+            lastBlinkTime = now;
+            ledState = !ledState;
+            digitalWrite(STATUS_LED_PIN, ledState ? STATUS_LED_ON : STATUS_LED_OFF);
+        }
+    } else if (isConnected) {
+        if (!ledState) {
+            digitalWrite(STATUS_LED_PIN, STATUS_LED_ON);
+            ledState = true;
+        }
+    } else {
+        if (ledState) {
+            digitalWrite(STATUS_LED_PIN, STATUS_LED_OFF);
+            ledState = false;
+        }
+    }
+#endif
+}

src/StatusLed.h

@@ -0,0 +1,12 @@
+#pragma once
+#include <Arduino.h>
+
+class StatusLed {
+public:
+    void init();
+    void update(bool isConnected, bool isMoving);
+
+private:
+    unsigned long lastBlinkTime = 0;
+    bool ledState = false;
+};

src/main.cpp

@@ -2,169 +2,32 @@
  * WijiBoard – BLE Stepper Controller Firmware
  * src/main.cpp
  *
- * Board  : Tenstar Robot ESP32-C3 Super Mini (or compatible ESP32-C3)
+ * Refactored modular architecture.
- * Steppers: Two 28BYJ-48 via ULN2003 (AccelStepper, FULL4WIRE mode)
- *
- * BLE Command Characteristic (WRITE_NR):
- *   S1+<n>   → Step motor 1 CW   n steps
- *   S1-<n>   → Step motor 1 CCW  n steps
- *   S2+<n>   → Step motor 2 CW   n steps
- *   S2-<n>   → Step motor 2 CCW  n steps
- *   SPD:<n>  → Set max speed for both motors (steps/sec)
- *   ACC:<n>  → Set acceleration for both motors (steps/sec²)
- *   HOME1    → Home motor 1 (Right)
- *   HOME2    → Home motor 2 (Left)
- *   HOMEALL  → Home both motors simultaneously
- *   DISABLE  → Disable steppers (un-hold) and require rehoming
- *   POS      → Request current positions (triggers NOTIFY)
- *
- * BLE Status Characteristic (NOTIFY):
- *   P:<s1>,<s2>  → Current step positions for motor 1 & 2
- *
- * UUIDs – must match web/js/ble.js exactly:
- *   Service  : a0b1c2d3-e4f5-6789-abcd-ef0123456700
- *   Command  : a0b1c2d3-e4f5-6789-abcd-ef0123456701
- *   Status   : a0b1c2d3-e4f5-6789-abcd-ef0123456702
  */
 
-#include <AccelStepper.h>
 #include <Arduino.h>
-#include <BLEDevice.h>
+#include "Config.h"
-#include <BLEServer.h>
+#include "BleManager.h"
-#include <BLEUtils.h>
+#include "MotorController.h"
+#include "StatusLed.h"
 
-// ─── Motor pin mapping (28BYJ-48 / ULN2003) ──────────────────────
+BleManager bleManager;
-// Motor 1 – Shoulder
+MotorController motorController;
-#define M1_IN1 0
+StatusLed statusLed;
-#define M1_IN2 1
-#define M1_IN3 3
-#define M1_IN4 4
 
-// Motor 2 – Elbow
-#define M2_IN1 5
-#define M2_IN2 6
-#define M2_IN3 7
-#define M2_IN4 10
-
-// ─── Stepper constants ────────────────────────────────────────────
-#define STEPS_PER_REV 2048
-#define DEFAULT_SPEED 600.0f // steps/sec
-#define DEFAULT_ACCEL 100.0f // steps/sec²
-float currentMaxSpeed = DEFAULT_SPEED;
-float currentAccel = DEFAULT_ACCEL;
-
-// ─── BLE UUIDs ────────────────────────────────────────────────────
-#define SERVICE_UUID "18f3b235-9831-4c75-8ec0-210469b820a0"
-#define CMD_UUID "cd083b06-4447-4cf3-a7c3-322ecf802ce4"
-#define STATUS_UUID "82e38c5b-d3ab-41d1-861c-b84dc6bb1e03"
-#define DEVICE_NAME "WijiBoard"
-
-// ─── Stepper objects ──────────────────────────────────────────────
-AccelStepper stepper1(AccelStepper::FULL4WIRE, M1_IN1, M1_IN2, M1_IN3, M1_IN4);
-AccelStepper stepper2(AccelStepper::FULL4WIRE, M2_IN1, M2_IN2, M2_IN3, M2_IN4);
-
-// ─── BLE globals ─────────────────────────────────────────────────
-BLEServer *pServer = nullptr;
-BLECharacteristic *pCmdChar = nullptr;
-BLECharacteristic *pStatusChar = nullptr;
-bool bleConnected = false;
-
-// ─── Status notify throttle ───────────────────────────────────────
 unsigned long lastNotify = 0;
-const unsigned long NOTIFY_INTERVAL_MS = 200;
 
-// ─── Idle timeout tracker ─────────────────────────────────────────
+void onCommandReceived(const String &cmd) {
-unsigned long lastMotorActive = 0;
-const unsigned long IDLE_TIMEOUT_MS = 30 * 60 * 1000; // 30 minutes
-bool motorsDisabled = true; // Assume disabled at startup until moved
-
-// ─── Homing State Machine ─────────────────────────────────────────
-enum HomingState {
-  HOME_IDLE,
-  HOME_ALL_PHASE1,
-  HOME_ALL_PHASE2,
-  HOME_1_PHASE1,
-  HOME_1_PHASE2,
-  HOME_2_PHASE1,
-  HOME_2_PHASE2
-};
-HomingState homingState = HOME_IDLE;
-
-// ─── LED globals ──────────────────────────────────────────────────
-#ifndef STATUS_LED_PIN
-  #ifdef LED_BUILTIN
-    #define STATUS_LED_PIN LED_BUILTIN
-  #endif
-#endif
-
-#ifndef STATUS_LED_ON
-  #define STATUS_LED_ON HIGH
-  #define STATUS_LED_OFF LOW
-#endif
-
-unsigned long lastBlinkTime = 0;
-bool ledState = false;
-
-// ─── Forward declarations ─────────────────────────────────────────
-void parseCommand(const String &cmd);
-void sendPosition();
-void performHomingAll();
-void performHoming1();
-void performHoming2();
-
-// ─── Homing Routines ──────────────────────────────────────────────
-
-void runSteppersWithNotify() {
-  stepper1.run();
-  stepper2.run();
-  if (bleConnected) {
-    unsigned long now = millis();
-    if (now - lastNotify >= NOTIFY_INTERVAL_MS) {
-      lastNotify = now;
-      sendPosition();
-    }
-  }
-}
-
-// ─── BLE Server Callbacks ─────────────────────────────────────────
-class ServerCallbacks : public BLEServerCallbacks {
-  void onConnect(BLEServer *) override {
-    bleConnected = true;
-    Serial.println("[BLE] Client connected");
-  }
-  void onDisconnect(BLEServer *) override {
-    bleConnected = false;
-    Serial.println("[BLE] Client disconnected – restarting advertising");
-    BLEDevice::startAdvertising();
-  }
-};
-
-// ─── Command Characteristic Callbacks ────────────────────────────
-class CmdCallbacks : public BLECharacteristicCallbacks {
-  void onWrite(BLECharacteristic *pChar) override {
-    String val = pChar->getValue().c_str();
-    val.trim();
-    if (val.length() == 0)
-      return;
-    Serial.printf("[CMD] Received: '%s'\n", val.c_str());
-    parseCommand(val);
-  }
-};
-
-// ─── Command parser ───────────────────────────────────────────────
-void parseCommand(const String &cmd) {
   // S1+<n> or S1-<n> (Relative)
   if (cmd.startsWith("S1") && cmd.length() > 2) {
     long steps = cmd.substring(2).toInt();
-    stepper1.move(steps);
+    motorController.moveRelative1(steps);
-    Serial.printf("[S1] Move relative %+ld steps\n", steps);
     return;
   }
   // S2+<n> or S2-<n> (Relative)
   if (cmd.startsWith("S2") && cmd.length() > 2) {
     long steps = cmd.substring(2).toInt();
-    stepper2.move(steps);
+    motorController.moveRelative2(steps);
-    Serial.printf("[S2] Move relative %+ld steps\n", steps);
     return;
   }
   // X:<n1>,<n2> (Absolute combined)
@@ -173,268 +36,93 @@ void parseCommand(const String &cmd) {
     if (commaIdx != -1) {
       long target1 = cmd.substring(2, commaIdx).toInt();
       long target2 = cmd.substring(commaIdx + 1).toInt();
-      stepper1.moveTo(target1);
+      motorController.moveAbsolute(target1, target2);
-      stepper2.moveTo(target2);
-      Serial.printf("[X] Move absolute to %ld, %ld\n", target1, target2);
     }
     return;
   }
   // SPD:<n>
   if (cmd.startsWith("SPD:")) {
     float spd = cmd.substring(4).toFloat();
-    currentMaxSpeed = spd;
+    motorController.setMaxSpeed(spd);
-    stepper1.setMaxSpeed(spd);
-    stepper2.setMaxSpeed(spd);
-    Serial.printf("[CFG] Max speed → %.0f steps/sec\n", spd);
     return;
   }
   // ACC:<n>
   if (cmd.startsWith("ACC:")) {
     float acc = cmd.substring(4).toFloat();
-    currentAccel = acc;
+    motorController.setAcceleration(acc);
-    stepper1.setAcceleration(acc);
-    stepper2.setAcceleration(acc);
-    Serial.printf("[CFG] Acceleration → %.0f steps/sec²\n", acc);
     return;
   }
   // HOME1
   if (cmd == "HOME1") {
-    Serial.println("[S1] Homing Motor 1...");
+    motorController.home1();
-    performHoming1();
+    bleManager.notifyPosition(motorController.getPositionString());
-    sendPosition();
     return;
   }
   // HOME2
   if (cmd == "HOME2") {
-    Serial.println("[S2] Homing Motor 2...");
+    motorController.home2();
-    performHoming2();
+    bleManager.notifyPosition(motorController.getPositionString());
-    sendPosition();
     return;
   }
   // HOMEALL
   if (cmd == "HOMEALL") {
-    Serial.println("[SYS] Homing both motors...");
+    motorController.homeAll();
-    performHomingAll();
+    bleManager.notifyPosition(motorController.getPositionString());
-    sendPosition();
     return;
   }
   // ZEROALL
   if (cmd == "ZEROALL") {
-    stepper1.setCurrentPosition(1024);
+    motorController.zeroAll();
-    stepper2.setCurrentPosition(0);
+    bleManager.notifyPosition(motorController.getPositionString());
-    Serial.println("[SYS] Position blindly reset to home (1024, 0).");
-    sendPosition();
     return;
   }
   // DISABLE
   if (cmd == "DISABLE") {
-    stepper1.disableOutputs();
+    motorController.disableMotors();
-    stepper2.disableOutputs();
-    motorsDisabled = true;
-    Serial.println("[SYS] Steppers manually disabled.");
     return;
   }
   // POS – explicit position request
   if (cmd == "POS") {
-    sendPosition();
+    bleManager.notifyPosition(motorController.getPositionString());
     return;
   }
   Serial.printf("[CMD] Unknown command: '%s'\n", cmd.c_str());
 }
 
-// ─── Send current positions via BLE NOTIFY ────────────────────────
+void onIdleTimeout() {
-void sendPosition() {
+    bleManager.notifyMessage("SYS:IDLE_TIMEOUT");
-  if (!bleConnected || !pStatusChar)
-    return;
-  bool isHolding = !motorsDisabled || stepper1.isRunning() || stepper2.isRunning() || homingState != HOME_IDLE;
-  String pos = "P:" + String(stepper1.currentPosition()) + "," +
-               String(stepper2.currentPosition()) + "," +
-               String(isHolding ? 1 : 0);
-  pStatusChar->setValue(pos.c_str());
-  pStatusChar->notify();
-  Serial.printf("[POS] %s\n", pos.c_str());
 }
 
-// ─── Setup ────────────────────────────────────────────────────────
 void setup() {
   Serial.setTxTimeoutMs(0);
   Serial.begin(115200);
   delay(1000); // Wait for USB CDC to enumerate
   Serial.println("\n[BOOT] WijiBoard Stepper Controller");
 
-#ifdef STATUS_LED_PIN
+  statusLed.init();
-  pinMode(STATUS_LED_PIN, OUTPUT);
+  motorController.init();
-  digitalWrite(STATUS_LED_PIN, STATUS_LED_OFF);
+  motorController.setOnTimeoutCallback(onIdleTimeout);
-#endif
+  bleManager.init(onCommandReceived);
-
-  // ── Init steppers ──────────────────────────────────────────
-  stepper1.setMaxSpeed(DEFAULT_SPEED);
-  stepper1.setAcceleration(DEFAULT_ACCEL);
-  stepper1.setCurrentPosition(0);
-
-  stepper2.setMaxSpeed(DEFAULT_SPEED);
-  stepper2.setAcceleration(DEFAULT_ACCEL);
-  stepper2.setCurrentPosition(0);
-
-  Serial.println("[STEP] Steppers initialized");
-
-  // ── Init BLE ───────────────────────────────────────────────
-  BLEDevice::init(DEVICE_NAME);
-
-  pServer = BLEDevice::createServer();
-  pServer->setCallbacks(new ServerCallbacks());
-
-  // Service
-  BLEService *pService = pServer->createService(SERVICE_UUID);
-
-  // Command characteristic (Write without response)
-  pCmdChar = pService->createCharacteristic(CMD_UUID,
-                                            BLECharacteristic::PROPERTY_WRITE);
-  pCmdChar->setCallbacks(new CmdCallbacks());
-
-  // Status characteristic (Notify)
-  pStatusChar = pService->createCharacteristic(
-      STATUS_UUID, BLECharacteristic::PROPERTY_NOTIFY);
-  // pStatusChar->addDescriptor(new BLE2902()); // Removed due to
-  // deprecation/NimBLE conflicts
-
-  pService->start();
-
-  // Advertising
-  BLEAdvertising *pAdv = BLEDevice::getAdvertising();
-  pAdv->addServiceUUID(SERVICE_UUID);
-  pAdv->setScanResponse(true);
-  pAdv->setMinPreferred(0x06); // 7.5ms
-  pAdv->setMaxPreferred(0x0C); // 15ms - CRITICAL for Linux/BlueZ stability
-  BLEDevice::startAdvertising();
-
-  Serial.printf("[BLE] Advertising as '%s' – ready!\n", DEVICE_NAME);
 }
 
-// ─── Loop ─────────────────────────────────────────────────────────
-
 void loop() {
-  stepper1.run();
+  static bool wasMoving = false;
-  stepper2.run();
-
-  bool moving = stepper1.isRunning() || stepper2.isRunning() || homingState != HOME_IDLE;
   
-  if (moving) {
+  motorController.update();
-    lastMotorActive = millis();
+  bool isMoving = motorController.isMoving();
-    motorsDisabled = false;
+  statusLed.update(bleManager.isConnected(), isMoving);
-  } else if (!motorsDisabled && (millis() - lastMotorActive > IDLE_TIMEOUT_MS)) {
-    stepper1.disableOutputs();
-    stepper2.disableOutputs();
-    motorsDisabled = true;
-    Serial.println("[SYS] Idle timeout reached. Steppers disabled.");
-    if (bleConnected && pStatusChar) {
-      pStatusChar->setValue("SYS:IDLE_TIMEOUT");
-      pStatusChar->notify();
-    }
-  }
 
-  if (homingState == HOME_ALL_PHASE1) {
+  if (bleManager.isConnected()) {
-    if (stepper1.distanceToGo() == 0 && stepper2.distanceToGo() == 0) {
-      stepper1.setCurrentPosition(1024);
-      stepper2.setCurrentPosition(0);
-      stepper1.moveTo(1024);
-      stepper2.moveTo(0);
-      homingState = HOME_ALL_PHASE2;
-      Serial.println("[SYS] Homing all: Phase 2 (Hold Outward Home)");
-    }
-  } else if (homingState == HOME_ALL_PHASE2) {
-    if (stepper1.distanceToGo() == 0 && stepper2.distanceToGo() == 0) {
-      stepper1.setMaxSpeed(currentMaxSpeed);
-      stepper2.setMaxSpeed(currentMaxSpeed);
-      stepper1.setAcceleration(currentAccel);
-      stepper2.setAcceleration(currentAccel);
-      sendPosition();
-      homingState = HOME_IDLE;
-      Serial.println("[SYS] Homing all complete.");
-    }
-  } else if (homingState == HOME_1_PHASE1) {
-    if (stepper1.distanceToGo() == 0) {
-      stepper1.setCurrentPosition(1024);
-      stepper1.moveTo(1024);
-      homingState = HOME_1_PHASE2;
-    }
-  } else if (homingState == HOME_1_PHASE2) {
-    if (stepper1.distanceToGo() == 0) {
-      stepper1.setMaxSpeed(currentMaxSpeed);
-      stepper1.setAcceleration(currentAccel);
-      homingState = HOME_IDLE;
-    }
-  } else if (homingState == HOME_2_PHASE1) {
-    if (stepper2.distanceToGo() == 0) {
-      stepper2.setCurrentPosition(0);
-      stepper2.moveTo(0);
-      homingState = HOME_2_PHASE2;
-    }
-  } else if (homingState == HOME_2_PHASE2) {
-    if (stepper2.distanceToGo() == 0) {
-      stepper2.setMaxSpeed(currentMaxSpeed);
-      stepper2.setAcceleration(currentAccel);
-      homingState = HOME_IDLE;
-    }
-  }
-
-  // Periodic position notify while motors are moving
-  if (bleConnected) {
     unsigned long now = millis();
-    if (moving && (now - lastNotify >= NOTIFY_INTERVAL_MS)) {
+    // Notify periodically while moving, or once when stopping
+    if (isMoving && (now - lastNotify >= NOTIFY_INTERVAL_MS)) {
       lastNotify = now;
-      sendPosition();
+      bleManager.notifyPosition(motorController.getPositionString());
+    } else if (wasMoving && !isMoving) {
+      // Just stopped moving
+      bleManager.notifyPosition(motorController.getPositionString());
     }
   }
-
+  
-  // LED state management
+  wasMoving = isMoving;
-#ifdef STATUS_LED_PIN
-  if (moving) {
-    unsigned long now = millis();
-    if (now - lastBlinkTime >= 150) { // 150ms blink interval
-      lastBlinkTime = now;
-      ledState = !ledState;
-      digitalWrite(STATUS_LED_PIN, ledState ? STATUS_LED_ON : STATUS_LED_OFF);
-    }
-  } else if (bleConnected) {
-    if (!ledState) {
-      digitalWrite(STATUS_LED_PIN, STATUS_LED_ON);
-      ledState = true;
-    }
-  } else {
-    if (ledState) {
-      digitalWrite(STATUS_LED_PIN, STATUS_LED_OFF);
-      ledState = false;
-    }
-  }
-#endif
-}
-
-// ─── Homing Routines ──────────────────────────────────────────────
-
-void performHomingAll() {
-  Serial.println("[SYS] Homing all: Phase 1 (Stall OUTWARD)");
-  stepper1.setMaxSpeed(1500);
-  stepper2.setMaxSpeed(1500);
-  stepper1.setAcceleration(1500);
-  stepper2.setAcceleration(1500);
-  stepper1.move(-2000); 
-  stepper2.move(2000);
-  homingState = HOME_ALL_PHASE1;
-}
-
-void performHoming1() {
-  Serial.println("[S1] Homing Motor 1: Phase 1 (Stall OUTWARD)");
-  stepper1.setMaxSpeed(1500);
-  stepper1.setAcceleration(1500);
-  stepper1.move(-2000);
-  homingState = HOME_1_PHASE1;
-}
-
-void performHoming2() {
-  Serial.println("[S2] Homing Motor 2: Phase 1 (Stall OUTWARD)");
-  stepper2.setMaxSpeed(1500);
-  stepper2.setAcceleration(1500);
-  stepper2.move(2000);
-  homingState = HOME_2_PHASE1;
 }