214 lines
7.2 KiB
Arduino
214 lines
7.2 KiB
Arduino
/*
|
|
* receiver.ino -- UART -> two LEGO Powered Up hubs (ESP32 "B")
|
|
*
|
|
* Board package: esp32 (the normal Espressif one)
|
|
* Libraries: Legoino + NimBLE-Arduino 1.4.x (both via Library Manager)
|
|
*
|
|
* Listens for gamepad frames from the Bluepad32 board on Serial2 and drives
|
|
* two Powered Up / Technic hubs over BLE using Legoino.
|
|
*
|
|
* Wiring to the transmitter board:
|
|
* RX GPIO16 <- TX GPIO17 on transmitter
|
|
* TX GPIO17 -> RX GPIO16 on transmitter
|
|
* GND -> GND
|
|
*
|
|
* Created by: Jess Rogerson (yelling commands at Claude.AI)
|
|
*/
|
|
|
|
#include "Lpf2Hub.h"
|
|
|
|
// ---------------------------------------------------------------- settings
|
|
|
|
// Hub BLE addresses. Run tools/hub_scanner to find them - do not guess.
|
|
// Lower case, colon separated.
|
|
static const char *HUB_TRACKS_ADDR = "90:84:2b:61:e6:8c";
|
|
static const char *HUB_UPPERBODY_ADDR = "90:84:2b:61:f2:d7";
|
|
|
|
// Port numbers are just bytes in the LEGO protocol, same on every hub type:
|
|
static const byte PORT_A = 0x00;
|
|
static const byte PORT_B = 0x01;
|
|
static const byte PORT_C = 0x02;
|
|
static const byte PORT_D = 0x03;
|
|
|
|
// Technic / Control+ motors (42100 etc.) are tacho motors -> leave this at 1.
|
|
// Plain train motors and the simple Powered Up motors -> set it to 0.
|
|
#define USE_TACHO_MOTORS 1
|
|
|
|
static const int DEADZONE = 40; // raw stick counts ignored around centre
|
|
static const int TRACK_MAX = 100; // LEGO speed range is -100..100
|
|
static const int HEAD_MAX = 60;
|
|
static const unsigned long MOTOR_MIN_INTERVAL_MS = 60; // per port throttle
|
|
static const unsigned long LINK_TIMEOUT_MS = 400; // failsafe
|
|
|
|
static const int LINK_RX_PIN = 16;
|
|
static const int LINK_TX_PIN = 17;
|
|
static const long LINK_BAUD = 115200;
|
|
|
|
static const int STATUS_LED_PIN = 2;
|
|
|
|
// ------------------------------------------------------------------ state
|
|
Lpf2Hub hubTracks;
|
|
Lpf2Hub hubUpperBody;
|
|
|
|
static bool tracksInitialised = false;
|
|
static bool upperInitialised = false;
|
|
static unsigned long tracksRetryAt = 0;
|
|
static unsigned long upperRetryAt = 0;
|
|
|
|
static unsigned long lastFrameAt = 0;
|
|
static bool failsafeEngaged = true;
|
|
|
|
struct PortState {
|
|
int lastSpeed;
|
|
unsigned long lastSentAt;
|
|
};
|
|
static PortState trackLeft = {0, 0};
|
|
static PortState trackRight = {0, 0};
|
|
static PortState headTurn = {0, 0};
|
|
|
|
// ----------------------------------------------------------------- helpers
|
|
|
|
// Apply a deadzone, then scale what is left so the usable travel still
|
|
// reaches full speed instead of jumping from 0 to a third of the range.
|
|
static int stickToSpeed(int raw, int maxSpeed) {
|
|
if (raw > -DEADZONE && raw < DEADZONE) return 0;
|
|
int sign = (raw < 0) ? -1 : 1;
|
|
long magnitude = abs((long)raw) - DEADZONE;
|
|
long span = 512L - DEADZONE;
|
|
long scaled = (magnitude * maxSpeed) / span;
|
|
if (scaled > maxSpeed) scaled = maxSpeed;
|
|
return sign * (int)scaled;
|
|
}
|
|
|
|
static void driveMotor(Lpf2Hub &hub, byte port, int speed, PortState &state) {
|
|
if (!hub.isConnected()) return;
|
|
|
|
unsigned long now = millis();
|
|
bool changed = (speed != state.lastSpeed);
|
|
bool stopping = (speed == 0 && state.lastSpeed != 0);
|
|
|
|
// Stops always go out immediately; everything else is throttled so we
|
|
// don't flood the hub's BLE queue and stall it.
|
|
if (!stopping && (!changed || (now - state.lastSentAt) < MOTOR_MIN_INTERVAL_MS)) return;
|
|
|
|
#if USE_TACHO_MOTORS
|
|
hub.setTachoMotorSpeed(port, speed);
|
|
#else
|
|
hub.setBasicMotorSpeed(port, speed);
|
|
#endif
|
|
|
|
state.lastSpeed = speed;
|
|
state.lastSentAt = now;
|
|
}
|
|
|
|
static void stopEverything() {
|
|
driveMotor(hubTracks, PORT_A, 0, trackLeft);
|
|
driveMotor(hubTracks, PORT_B, 0, trackRight);
|
|
driveMotor(hubUpperBody, PORT_A, 0, headTurn);
|
|
}
|
|
|
|
// Connect the hubs one at a time. Kicking off two scans at once upsets the
|
|
// shared NimBLE scanner and you end up with one hub connected and one sulking.
|
|
static void serviceHub(Lpf2Hub &hub, const char *address, bool &initialised,
|
|
unsigned long &retryAt, const char *label) {
|
|
if (hub.isConnected()) return;
|
|
|
|
if (hub.isConnecting()) {
|
|
hub.connectHub();
|
|
if (hub.isConnected()) {
|
|
Serial.printf("[%s] connected\n", label);
|
|
hub.setLedColor(GREEN);
|
|
} else {
|
|
Serial.printf("[%s] connect failed, retrying\n", label);
|
|
initialised = false;
|
|
retryAt = millis() + 2000;
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (!initialised && millis() >= retryAt) {
|
|
Serial.printf("[%s] scanning for %s\n", label, address);
|
|
hub.init(std::string(address));
|
|
initialised = true;
|
|
}
|
|
}
|
|
|
|
static uint8_t xorChecksum(const char *s, size_t len) {
|
|
uint8_t c = 0;
|
|
for (size_t i = 0; i < len; i++) c ^= (uint8_t)s[i];
|
|
return c;
|
|
}
|
|
|
|
// Returns true if a valid frame was parsed.
|
|
static bool parseFrame(char *line, int &lx, int &ly, int &rx, int &ry, unsigned &buttons) {
|
|
char *star = strrchr(line, '*');
|
|
if (!star) return false;
|
|
*star = '\0';
|
|
|
|
unsigned expected = 0;
|
|
if (sscanf(star + 1, "%2x", &expected) != 1) return false;
|
|
if (xorChecksum(line, strlen(line)) != (uint8_t)expected) return false;
|
|
|
|
return sscanf(line, "G,%d,%d,%d,%d,%u", &lx, &ly, &rx, &ry, &buttons) == 5;
|
|
}
|
|
|
|
// -------------------------------------------------------------------- main
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
Serial2.begin(LINK_BAUD, SERIAL_8N1, LINK_RX_PIN, LINK_TX_PIN);
|
|
|
|
pinMode(STATUS_LED_PIN, OUTPUT);
|
|
digitalWrite(STATUS_LED_PIN, LOW);
|
|
|
|
Serial.println("LEGO hub receiver starting");
|
|
}
|
|
|
|
void loop() {
|
|
// 1. Keep the hubs connected, tracks first.
|
|
serviceHub(hubTracks, HUB_TRACKS_ADDR, tracksInitialised, tracksRetryAt, "tracks");
|
|
if (hubTracks.isConnected()) {
|
|
serviceHub(hubUpperBody, HUB_UPPERBODY_ADDR, upperInitialised, upperRetryAt, "upper");
|
|
}
|
|
|
|
digitalWrite(STATUS_LED_PIN,
|
|
(hubTracks.isConnected() && hubUpperBody.isConnected()) ? HIGH : LOW);
|
|
|
|
// 2. Pull whole lines off the link.
|
|
static char buf[96];
|
|
static size_t idx = 0;
|
|
|
|
while (Serial2.available()) {
|
|
char c = (char)Serial2.read();
|
|
if (c == '\r') continue;
|
|
if (c == '\n') {
|
|
buf[idx] = '\0';
|
|
int lx, ly, rx, ry;
|
|
unsigned buttons;
|
|
if (idx > 0 && parseFrame(buf, lx, ly, rx, ry, buttons)) {
|
|
lastFrameAt = millis();
|
|
failsafeEngaged = false;
|
|
|
|
int leftTrack = stickToSpeed(-ly, TRACK_MAX); // push forward = positive
|
|
int rightTrack = stickToSpeed(-ry, TRACK_MAX);
|
|
int head = stickToSpeed(lx, HEAD_MAX);
|
|
|
|
driveMotor(hubTracks, PORT_A, leftTrack, trackLeft);
|
|
driveMotor(hubTracks, PORT_B, rightTrack, trackRight);
|
|
driveMotor(hubUpperBody, PORT_A, head, headTurn);
|
|
}
|
|
idx = 0;
|
|
} else if (idx < sizeof(buf) - 1) {
|
|
buf[idx++] = c;
|
|
} else {
|
|
idx = 0; // overrun, throw the line away
|
|
}
|
|
}
|
|
|
|
// 3. Failsafe - link went quiet, stop before something drives off a table.
|
|
if (!failsafeEngaged && (millis() - lastFrameAt) > LINK_TIMEOUT_MS) {
|
|
Serial.println("Link timeout - stopping motors");
|
|
stopEverything();
|
|
failsafeEngaged = true;
|
|
}
|
|
}
|