Initial commit
This commit is contained in:
PROFERIS - Mi³osz Stocki
+235
@@ -0,0 +1,235 @@
|
||||
/**
|
||||
* WijiBoard – BLE Stepper Controller Firmware
|
||||
* src/main.cpp
|
||||
*
|
||||
* Board : Waveshare ESP32-S3-Zero (or compatible ESP32-S3)
|
||||
* 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 → Zero motor 1 position
|
||||
* HOME2 → Zero motor 2 position
|
||||
* 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 <Arduino.h>
|
||||
#include <AccelStepper.h>
|
||||
#include <BLEDevice.h>
|
||||
#include <BLEServer.h>
|
||||
#include <BLEUtils.h>
|
||||
#include <BLE2902.h>
|
||||
|
||||
// ─── Motor pin mapping (28BYJ-48 / ULN2003) ──────────────────────
|
||||
// Motor 1 – Shoulder
|
||||
#define M1_IN1 4
|
||||
#define M1_IN2 5
|
||||
#define M1_IN3 6
|
||||
#define M1_IN4 7
|
||||
|
||||
// Motor 2 – Elbow
|
||||
#define M2_IN1 8
|
||||
#define M2_IN2 9
|
||||
#define M2_IN3 10
|
||||
#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²
|
||||
|
||||
// ─── 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"
|
||||
|
||||
// ─── 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;
|
||||
|
||||
// ─── Forward declarations ─────────────────────────────────────────
|
||||
void parseCommand(const String& cmd);
|
||||
void 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>
|
||||
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") {
|
||||
stepper1.setCurrentPosition(0);
|
||||
Serial.println("[S1] Zeroed");
|
||||
sendPosition();
|
||||
return;
|
||||
}
|
||||
// HOME2
|
||||
if (cmd == "HOME2") {
|
||||
stepper2.setCurrentPosition(0);
|
||||
Serial.println("[S2] Zeroed");
|
||||
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());
|
||||
}
|
||||
|
||||
// ─── 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");
|
||||
|
||||
// ── 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 |
|
||||
BLECharacteristic::PROPERTY_WRITE_NR
|
||||
);
|
||||
pCmdChar->setCallbacks(new CmdCallbacks());
|
||||
|
||||
// Status characteristic (Notify)
|
||||
pStatusChar = pService->createCharacteristic(
|
||||
STATUS_UUID,
|
||||
BLECharacteristic::PROPERTY_NOTIFY
|
||||
);
|
||||
pStatusChar->addDescriptor(new BLE2902());
|
||||
|
||||
pService->start();
|
||||
|
||||
// Advertising
|
||||
BLEAdvertising* pAdv = BLEDevice::getAdvertising();
|
||||
pAdv->addServiceUUID(SERVICE_UUID);
|
||||
pAdv->setScanResponse(true);
|
||||
pAdv->setMinPreferred(0x06);
|
||||
BLEDevice::startAdvertising();
|
||||
|
||||
Serial.printf("[BLE] Advertising as '%s' – ready!\n", DEVICE_NAME);
|
||||
}
|
||||
|
||||
// ─── Loop ─────────────────────────────────────────────────────────
|
||||
void loop() {
|
||||
// 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();
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user