diff --git a/receiver/receiver.ino b/receiver/receiver.ino new file mode 100644 index 0000000..cd4925c --- /dev/null +++ b/receiver/receiver.ino @@ -0,0 +1,213 @@ +/* + * 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; + } +}