Refactor mcu code. Closes #11
This commit is contained in:
osiu97
@@ -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.
|
||||
|
||||
---
|
||||
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
@@ -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;
|
||||
};
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
};
|
||||
@@ -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
|
||||
}
|
||||
@@ -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;
|
||||
};
|
||||
+43
-355
@@ -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+<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
|
||||
* Refactored modular architecture.
|
||||
*/
|
||||
|
||||
#include <AccelStepper.h>
|
||||
#include <Arduino.h>
|
||||
#include <BLEDevice.h>
|
||||
#include <BLEServer.h>
|
||||
#include <BLEUtils.h>
|
||||
#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+<n> or S1-<n> (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+<n> or S2-<n> (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:<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);
|
||||
stepper2.moveTo(target2);
|
||||
Serial.printf("[X] Move absolute to %ld, %ld\n", target1, target2);
|
||||
motorController.moveAbsolute(target1, target2);
|
||||
}
|
||||
return;
|
||||
}
|
||||
// SPD:<n>
|
||||
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:<n>
|
||||
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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user