From 33436fafc4855ff976a173a947df7f6c59844e7e Mon Sep 17 00:00:00 2001 From: osiu97 Date: Wed, 8 Jul 2026 20:25:20 +0200 Subject: [PATCH] Refactor mcu code. Closes #11 --- gemini.md | 8 +- src/BleManager.cpp | 84 +++++++++ src/BleManager.h | 23 +++ src/Config.h | 40 ++++ src/MotorController.cpp | 180 ++++++++++++++++++ src/MotorController.h | 54 ++++++ src/StatusLed.cpp | 32 ++++ src/StatusLed.h | 12 ++ src/main.cpp | 398 +++++----------------------------------- 9 files changed, 474 insertions(+), 357 deletions(-) create mode 100644 src/BleManager.cpp create mode 100644 src/BleManager.h create mode 100644 src/Config.h create mode 100644 src/MotorController.cpp create mode 100644 src/MotorController.h create mode 100644 src/StatusLed.cpp create mode 100644 src/StatusLed.h diff --git a/gemini.md b/gemini.md index 96a985d..6ffbdef 100644 --- a/gemini.md +++ b/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. --- diff --git a/src/BleManager.cpp b/src/BleManager.cpp new file mode 100644 index 0000000..2a5b7fc --- /dev/null +++ b/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 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()); +} diff --git a/src/BleManager.h b/src/BleManager.h new file mode 100644 index 0000000..dc678d9 --- /dev/null +++ b/src/BleManager.h @@ -0,0 +1,23 @@ +#pragma once +#include +#include +#include +#include + +class BleManager { +public: + BleManager(); + void init(std::function 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 onCommand; + bool connected = false; + BLECharacteristic *pStatusChar = nullptr; +}; diff --git a/src/Config.h b/src/Config.h new file mode 100644 index 0000000..49bc3cb --- /dev/null +++ b/src/Config.h @@ -0,0 +1,40 @@ +#pragma once +#include + +// ─── 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 diff --git a/src/MotorController.cpp b/src/MotorController.cpp new file mode 100644 index 0000000..096791f --- /dev/null +++ b/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 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; + } + } +} diff --git a/src/MotorController.h b/src/MotorController.h new file mode 100644 index 0000000..cbca075 --- /dev/null +++ b/src/MotorController.h @@ -0,0 +1,54 @@ +#pragma once +#include +#include +#include + +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 cb); + + bool isMoving(); + String getPositionString(); + +private: + AccelStepper stepper1; + AccelStepper stepper2; + + float currentMaxSpeed; + float currentAccel; + + unsigned long lastMotorActive; + bool motorsDisabled; + std::function 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(); +}; diff --git a/src/StatusLed.cpp b/src/StatusLed.cpp new file mode 100644 index 0000000..7cdd086 --- /dev/null +++ b/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 +} diff --git a/src/StatusLed.h b/src/StatusLed.h new file mode 100644 index 0000000..df1a11f --- /dev/null +++ b/src/StatusLed.h @@ -0,0 +1,12 @@ +#pragma once +#include + +class StatusLed { +public: + void init(); + void update(bool isConnected, bool isMoving); + +private: + unsigned long lastBlinkTime = 0; + bool ledState = false; +}; diff --git a/src/main.cpp b/src/main.cpp index 5650580..b1fec92 100644 --- a/src/main.cpp +++ b/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) - * Steppers: Two 28BYJ-48 via ULN2003 (AccelStepper, FULL4WIRE mode) - * - * BLE Command Characteristic (WRITE_NR): - * S1+ → Step motor 1 CW n steps - * S1- → Step motor 1 CCW n steps - * S2+ → Step motor 2 CW n steps - * S2- → Step motor 2 CCW n steps - * SPD: → Set max speed for both motors (steps/sec) - * ACC: → 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:, → 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 + * Refactored modular architecture. */ -#include #include -#include -#include -#include +#include "Config.h" +#include "BleManager.h" +#include "MotorController.h" +#include "StatusLed.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 +BleManager bleManager; +MotorController motorController; +StatusLed statusLed; -// 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 ───────────────────────────────────────── -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) { +void onCommandReceived(const String &cmd) { // S1+ or S1- (Relative) if (cmd.startsWith("S1") && cmd.length() > 2) { long steps = cmd.substring(2).toInt(); - stepper1.move(steps); - Serial.printf("[S1] Move relative %+ld steps\n", steps); + motorController.moveRelative1(steps); return; } // S2+ or S2- (Relative) if (cmd.startsWith("S2") && cmd.length() > 2) { long steps = cmd.substring(2).toInt(); - stepper2.move(steps); - Serial.printf("[S2] Move relative %+ld steps\n", steps); + motorController.moveRelative2(steps); return; } // X:, (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); - stepper2.moveTo(target2); - Serial.printf("[X] Move absolute to %ld, %ld\n", target1, target2); + motorController.moveAbsolute(target1, target2); } return; } // SPD: if (cmd.startsWith("SPD:")) { float spd = cmd.substring(4).toFloat(); - currentMaxSpeed = spd; - stepper1.setMaxSpeed(spd); - stepper2.setMaxSpeed(spd); - Serial.printf("[CFG] Max speed → %.0f steps/sec\n", spd); + motorController.setMaxSpeed(spd); return; } // ACC: if (cmd.startsWith("ACC:")) { float acc = cmd.substring(4).toFloat(); - currentAccel = acc; - stepper1.setAcceleration(acc); - stepper2.setAcceleration(acc); - Serial.printf("[CFG] Acceleration → %.0f steps/sec²\n", acc); + motorController.setAcceleration(acc); return; } // HOME1 if (cmd == "HOME1") { - Serial.println("[S1] Homing Motor 1..."); - performHoming1(); - sendPosition(); + motorController.home1(); + bleManager.notifyPosition(motorController.getPositionString()); return; } // HOME2 if (cmd == "HOME2") { - Serial.println("[S2] Homing Motor 2..."); - performHoming2(); - sendPosition(); + motorController.home2(); + bleManager.notifyPosition(motorController.getPositionString()); return; } // HOMEALL if (cmd == "HOMEALL") { - Serial.println("[SYS] Homing both motors..."); - performHomingAll(); - sendPosition(); + motorController.homeAll(); + bleManager.notifyPosition(motorController.getPositionString()); return; } // ZEROALL if (cmd == "ZEROALL") { - stepper1.setCurrentPosition(1024); - stepper2.setCurrentPosition(0); - Serial.println("[SYS] Position blindly reset to home (1024, 0)."); - sendPosition(); + motorController.zeroAll(); + bleManager.notifyPosition(motorController.getPositionString()); return; } // DISABLE if (cmd == "DISABLE") { - stepper1.disableOutputs(); - stepper2.disableOutputs(); - motorsDisabled = true; - Serial.println("[SYS] Steppers manually disabled."); + motorController.disableMotors(); 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 sendPosition() { - 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()); +void onIdleTimeout() { + bleManager.notifyMessage("SYS:IDLE_TIMEOUT"); } -// ─── 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 - pinMode(STATUS_LED_PIN, OUTPUT); - digitalWrite(STATUS_LED_PIN, STATUS_LED_OFF); -#endif - - // ── 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); + statusLed.init(); + motorController.init(); + motorController.setOnTimeoutCallback(onIdleTimeout); + bleManager.init(onCommandReceived); } -// ─── Loop ───────────────────────────────────────────────────────── - void loop() { - stepper1.run(); - stepper2.run(); - - bool moving = stepper1.isRunning() || stepper2.isRunning() || homingState != HOME_IDLE; + static bool wasMoving = false; - if (moving) { - lastMotorActive = millis(); - motorsDisabled = false; - } 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(); - } - } + motorController.update(); + bool isMoving = motorController.isMoving(); + statusLed.update(bleManager.isConnected(), isMoving); - 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); - 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) { + if (bleManager.isConnected()) { 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 -#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; + + wasMoving = isMoving; }