osiu97/ESP32-WijiBoard
1
0
Fork 0
Code Issues 10 Pull Requests Packages Projects Releases Wiki Activity

Working BLE on S3

Browse Source
This commit is contained in:
osiu97
2026-07-06 19:26:50 +02:00
parent 2e3780cb0b
commit ea4530a5e4
7 changed files with 327 additions and 239 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main.cpp
+202 -203
View File
@@ -26,12 +26,12 @@
* Status : a0b1c2d3-e4f5-6789-abcd-ef0123456702
*/
#include <Arduino.h>
#include <AccelStepper.h>
#include <Arduino.h>
#include <BLE2902.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
// ─── Motor pin mapping (28BYJ-48 / ULN2003) ──────────────────────
// Motor 1 – Shoulder
@@ -47,32 +47,32 @@
#define M2_IN4 11
// ─── Stepper constants ────────────────────────────────────────────
#define STEPS_PER_REV 2048
#define DEFAULT_SPEED 600.0f // steps/sec
#define DEFAULT_ACCEL 100.0f // steps/sec²
#define STEPS_PER_REV 2048
#define DEFAULT_SPEED 600.0f // steps/sec
#define DEFAULT_ACCEL 100.0f // steps/sec²
// ─── BLE UUIDs ────────────────────────────────────────────────────
#define SERVICE_UUID "a0b1c2d3-e4f5-6789-abcd-ef0123456700"
#define CMD_UUID "a0b1c2d3-e4f5-6789-abcd-ef0123456701"
#define STATUS_UUID "a0b1c2d3-e4f5-6789-abcd-ef0123456702"
#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"
#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;
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;
// ─── Forward declarations ─────────────────────────────────────────
void parseCommand(const String& cmd);
void parseCommand(const String &cmd);
void sendPosition();
void performHomingAll();
void performHoming1();
@@ -80,241 +80,240 @@ void performHoming2();
// ─── 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();
}
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);
}
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>
if (cmd.startsWith("S1") && cmd.length() > 2) {
long steps = cmd.substring(2).toInt(); // '+' or '-' prefix handled by toInt()
stepper1.move(steps);
Serial.printf("[S1] Move %+ld steps\n", steps);
return;
}
// S2+<n> or S2-<n>
if (cmd.startsWith("S2") && cmd.length() > 2) {
long steps = cmd.substring(2).toInt();
stepper2.move(steps);
Serial.printf("[S2] Move %+ld steps\n", steps);
return;
}
// SPD:<n>
if (cmd.startsWith("SPD:")) {
float spd = cmd.substring(4).toFloat();
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();
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...");
performHoming1();
sendPosition();
return;
}
// HOME2
if (cmd == "HOME2") {
Serial.println("[S2] Homing Motor 2...");
performHoming2();
sendPosition();
return;
}
// HOMEALL
if (cmd == "HOMEALL") {
Serial.println("[SYS] Homing both motors...");
performHomingAll();
sendPosition();
return;
}
// POS – explicit position request
if (cmd == "POS") {
sendPosition();
return;
}
Serial.printf("[CMD] Unknown command: '%s'\n", cmd.c_str());
void parseCommand(const String &cmd) {
// S1+<n> or S1-<n>
if (cmd.startsWith("S1") && cmd.length() > 2) {
long steps =
cmd.substring(2).toInt(); // '+' or '-' prefix handled by toInt()
stepper1.move(steps);
Serial.printf("[S1] Move %+ld steps\n", steps);
return;
}
// S2+<n> or S2-<n>
if (cmd.startsWith("S2") && cmd.length() > 2) {
long steps = cmd.substring(2).toInt();
stepper2.move(steps);
Serial.printf("[S2] Move %+ld steps\n", steps);
return;
}
// SPD:<n>
if (cmd.startsWith("SPD:")) {
float spd = cmd.substring(4).toFloat();
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();
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...");
performHoming1();
sendPosition();
return;
}
// HOME2
if (cmd == "HOME2") {
Serial.println("[S2] Homing Motor 2...");
performHoming2();
sendPosition();
return;
}
// HOMEALL
if (cmd == "HOMEALL") {
Serial.println("[SYS] Homing both motors...");
performHomingAll();
sendPosition();
return;
}
// POS – explicit position request
if (cmd == "POS") {
sendPosition();
return;
}
Serial.printf("[CMD] Unknown command: '%s'\n", cmd.c_str());
}
// ─── Send current positions via BLE NOTIFY ────────────────────────
void sendPosition() {
if (!bleConnected || !pStatusChar) return;
String pos = "P:" + String(stepper1.currentPosition()) +
"," + String(stepper2.currentPosition());
pStatusChar->setValue(pos.c_str());
pStatusChar->notify();
Serial.printf("[POS] %s\n", pos.c_str());
if (!bleConnected || !pStatusChar)
return;
String pos = "P:" + String(stepper1.currentPosition()) + "," +
String(stepper2.currentPosition());
pStatusChar->setValue(pos.c_str());
pStatusChar->notify();
Serial.printf("[POS] %s\n", pos.c_str());
}
// ─── Setup ────────────────────────────────────────────────────────
void setup() {
// Fix: zero TX buffer to eliminate CDC post-write stall
Serial.setTxBufferSize(0);
Serial.begin(115200);
delay(500);
Serial.println("\n[BOOT] WijiBoard Stepper Controller");
Serial.setTxTimeoutMs(0);
Serial.begin(115200);
delay(1000); // Wait for USB CDC to enumerate
Serial.println("\n[BOOT] WijiBoard Stepper Controller");
// ── Init steppers ──────────────────────────────────────────
stepper1.setMaxSpeed(DEFAULT_SPEED);
stepper1.setAcceleration(DEFAULT_ACCEL);
stepper1.setCurrentPosition(0);
// ── Init steppers ──────────────────────────────────────────
stepper1.setMaxSpeed(DEFAULT_SPEED);
stepper1.setAcceleration(DEFAULT_ACCEL);
stepper1.setCurrentPosition(0);
stepper2.setMaxSpeed(DEFAULT_SPEED);
stepper2.setAcceleration(DEFAULT_ACCEL);
stepper2.setCurrentPosition(0);
stepper2.setMaxSpeed(DEFAULT_SPEED);
stepper2.setAcceleration(DEFAULT_ACCEL);
stepper2.setCurrentPosition(0);
Serial.println("[STEP] Steppers initialized");
Serial.println("[STEP] Steppers initialized");
// ── Init BLE ───────────────────────────────────────────────
BLEDevice::init(DEVICE_NAME);
// ── Init BLE ───────────────────────────────────────────────
BLEDevice::init(DEVICE_NAME);
pServer = BLEDevice::createServer();
pServer->setCallbacks(new ServerCallbacks());
pServer = BLEDevice::createServer();
pServer->setCallbacks(new ServerCallbacks());
// Service
BLEService* pService = pServer->createService(SERVICE_UUID);
// Service
BLEService *pService = pServer->createService(SERVICE_UUID);
// Command characteristic (Write without response)
pCmdChar = pService->createCharacteristic(
CMD_UUID,
BLECharacteristic::PROPERTY_WRITE |
BLECharacteristic::PROPERTY_WRITE_NR
);
pCmdChar->setCallbacks(new CmdCallbacks());
// 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());
// Status characteristic (Notify)
pStatusChar = pService->createCharacteristic(
STATUS_UUID, BLECharacteristic::PROPERTY_NOTIFY);
// pStatusChar->addDescriptor(new BLE2902()); // Removed due to
// deprecation/NimBLE conflicts
pService->start();
pService->start();
// Advertising
BLEAdvertising* pAdv = BLEDevice::getAdvertising();
pAdv->addServiceUUID(SERVICE_UUID);
pAdv->setScanResponse(true);
pAdv->setMinPreferred(0x06);
BLEDevice::startAdvertising();
// 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);
Serial.printf("[BLE] Advertising as '%s' – ready!\n", DEVICE_NAME);
}
// ─── Loop ─────────────────────────────────────────────────────────
void loop() {
// Run steppers (non-blocking AccelStepper)
stepper1.run();
stepper2.run();
// Run steppers (non-blocking AccelStepper)
stepper1.run();
stepper2.run();
// Periodic position notify while motors are moving
if (bleConnected) {
unsigned long now = millis();
bool moving = stepper1.isRunning() || stepper2.isRunning();
if (moving && (now - lastNotify >= NOTIFY_INTERVAL_MS)) {
lastNotify = now;
sendPosition();
}
// Periodic position notify while motors are moving
if (bleConnected) {
unsigned long now = millis();
bool moving = stepper1.isRunning() || stepper2.isRunning();
if (moving && (now - lastNotify >= NOTIFY_INTERVAL_MS)) {
lastNotify = now;
sendPosition();
}
}
}
// ─── Homing Routines ──────────────────────────────────────────────
void performHomingAll() {
// 1. Move against limits (M1 CCW, M2 CCW)
stepper2.moveTo(1024);
stepper1.moveTo(2048);
while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
stepper1.run();
stepper2.run();
}
stepper1.setCurrentPosition(0);
// 1. Move against limits (M1 CCW, M2 CCW)
stepper2.moveTo(1024);
stepper1.moveTo(2048);
while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
stepper1.run();
stepper2.run();
}
stepper1.setCurrentPosition(0);
// 2. Move to negative limits (CW)
stepper2.moveTo(-1050);
stepper1.move(-1300); // equivalent to moveTo(-1300) since pos is 0
while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
stepper1.run();
stepper2.run();
}
stepper2.setCurrentPosition(0);
// 2. Move to negative limits (CW)
stepper2.moveTo(-1050);
stepper1.move(-1300); // equivalent to moveTo(-1300) since pos is 0
while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
stepper1.run();
stepper2.run();
}
stepper2.setCurrentPosition(0);
// 3. Move to final home position
stepper2.moveTo(550);
stepper1.moveTo(-530);
while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
stepper1.run();
stepper2.run();
}
// 4. Set origin relative to this final position
stepper2.setCurrentPosition(-1024);
stepper1.setCurrentPosition(0);
// 3. Move to final home position
stepper2.moveTo(550);
stepper1.moveTo(-530);
while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) {
stepper1.run();
stepper2.run();
}
stepper1.disableOutputs();
stepper2.disableOutputs();
Serial.println("[SYS] Homing all complete.");
// 4. Set origin relative to this final position
stepper2.setCurrentPosition(-1024);
stepper1.setCurrentPosition(0);
stepper1.disableOutputs();
stepper2.disableOutputs();
Serial.println("[SYS] Homing all complete.");
}
void performHoming1() {
// M1 (Right) stalls going positive (CCW)
stepper1.move(2048);
while (stepper1.distanceToGo() != 0) {
stepper1.run();
}
stepper1.setCurrentPosition(0);
// M1 (Right) stalls going positive (CCW)
stepper1.move(2048);
while (stepper1.distanceToGo() != 0) {
stepper1.run();
}
stepper1.setCurrentPosition(0);
// Move to final home position (CW)
stepper1.moveTo(-530);
while (stepper1.distanceToGo() != 0) {
stepper1.run();
}
stepper1.setCurrentPosition(0);
stepper1.disableOutputs();
Serial.println("[S1] Homing complete.");
// Move to final home position (CW)
stepper1.moveTo(-530);
while (stepper1.distanceToGo() != 0) {
stepper1.run();
}
stepper1.setCurrentPosition(0);
stepper1.disableOutputs();
Serial.println("[S1] Homing complete.");
}
void performHoming2() {
// M2 (Left) stalls going negative (CW)
stepper2.move(-2048);
while (stepper2.distanceToGo() != 0) {
stepper2.run();
}
stepper2.setCurrentPosition(0);
// M2 (Left) stalls going negative (CW)
stepper2.move(-2048);
while (stepper2.distanceToGo() != 0) {
stepper2.run();
}
stepper2.setCurrentPosition(0);
// Move to final home position (CCW)
stepper2.moveTo(550);
while (stepper2.distanceToGo() != 0) {
stepper2.run();
}
stepper2.setCurrentPosition(-1024);
stepper2.disableOutputs();
Serial.println("[S2] Homing complete.");
// Move to final home position (CCW)
stepper2.moveTo(550);
while (stepper2.distanceToGo() != 0) {
stepper2.run();
}
stepper2.setCurrentPosition(-1024);
stepper2.disableOutputs();
Serial.println("[S2] Homing complete.");
}