From 0b32384578b1dc49b070712ce5ef91f44342ac33 Mon Sep 17 00:00:00 2001 From: jessikitty Date: Wed, 9 Sep 2026 19:29:12 +1000 Subject: [PATCH] Rewrite receiver for all 7 Johnny 5 motors with tank drive - Parse the extended frame (d-pad and analog triggers). - Drive both hubs across all seven ports, one input per motor. - Add a per-hub rate limit alongside the per-port one; hub1 has four motors and the per-port throttle alone floods it. - DEBUG_MOTORS flag to log which port each command lands on. --- receiver/receiver.ino | 267 ++++++++++++++++++++++++++++-------------- 1 file changed, 181 insertions(+), 86 deletions(-) diff --git a/receiver/receiver.ino b/receiver/receiver.ino index cd4925c..3e5b3f2 100644 --- a/receiver/receiver.ino +++ b/receiver/receiver.ino @@ -1,16 +1,22 @@ /* * receiver.ino -- UART -> two LEGO Powered Up hubs (ESP32 "B") * + * Model: Johnny 5 (Short Circuit) MOC, 7 motors across 2 Technic hubs. + * * 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. + * both hubs over BLE using Legoino. This board must NOT have the Bluepad32 + * board package selected - keeping BTstack and NimBLE on separate chips is + * the entire reason there are two boards. * * Wiring to the transmitter board: * RX GPIO16 <- TX GPIO17 on transmitter * TX GPIO17 -> RX GPIO16 on transmitter - * GND -> GND + * GND -> GND (mandatory - common ground) + * + * Control scheme is tank drive: every input drives exactly one motor. * * Created by: Jess Rogerson (yelling commands at Claude.AI) */ @@ -20,116 +26,174 @@ // ---------------------------------------------------------------- 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"; +// +// hub 0 - lower body hub 1 - upper body +// A right track A head tilt +// B left track B head turn +// D body lift C left arm +// D right arm +static const char *HUB0_ADDR = "90:84:2b:61:e6:8c"; +static const char *HUB1_ADDR = "90:84:2b:61:f2:d7"; -// Port numbers are just bytes in the LEGO protocol, same on every hub type: +// 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. +// Technic / Control+ motors 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 +// Set to 1 to log every motor command that goes out. Useful for proving which +// port a command actually lands on. Noisy - turn it back off afterwards. +#define DEBUG_MOTORS 0 + +static const int DEADZONE = 40; // raw stick counts ignored around centre + +// Per-axis power caps. Everything except the tracks runs into a mechanical end +// stop and there is no position feedback, so holding a direction at a stop +// stalls the motor. Lower these if an axis feels forceful. +static const int TRACK_MAX = 100; // LEGO speed range is -100..100 +static const int HEAD_MAX = 45; +static const int LIFT_MAX = 60; +static const int ARM_MAX = 45; + +// Track scaling: normal, L1 held (precision), R1 held (full). +static const int SCALE_NORMAL = 75; +static const int SCALE_PRECISION = 40; +static const int SCALE_FULL = 100; + +// Bluepad32 button masks. Verify against your own pad with DEBUG_BUTTONS in +// the transmitter sketch if any of these seem wrong. +static const unsigned BTN_A = 0x0001; // Cross +static const unsigned BTN_B = 0x0002; // Circle +static const unsigned BTN_X = 0x0004; // Square +static const unsigned BTN_Y = 0x0008; // Triangle +static const unsigned BTN_L1 = 0x0010; +static const unsigned BTN_R1 = 0x0020; + +static const unsigned DPAD_U = 0x01; +static const unsigned DPAD_D = 0x02; +static const unsigned DPAD_R = 0x04; +static const unsigned DPAD_L = 0x08; + +static const unsigned long MOTOR_MIN_GAP_MS = 100; // per port +static const unsigned long HUB_MIN_GAP_MS = 25; // per hub, ~40 cmd/sec +static const unsigned long LINK_TIMEOUT_MS = 400; // failsafe +static const unsigned long RECONNECT_GAP_MS = 2000; 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; +static const int STATUS_LED_PIN = 2; -// ------------------------------------------------------------------ state -Lpf2Hub hubTracks; -Lpf2Hub hubUpperBody; +// =================================================================== types -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 HubLink { + Lpf2Hub hub; + const char *addr; + const char *label; + bool initialised; + unsigned long retryAt; + unsigned long lastCmdAt; // per-hub rate limit, shared across its ports +}; struct PortState { int lastSpeed; unsigned long lastSentAt; }; -static PortState trackLeft = {0, 0}; -static PortState trackRight = {0, 0}; -static PortState headTurn = {0, 0}; -// ----------------------------------------------------------------- helpers +// ================================================================= globals -// 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 HubLink gHubs[2] = { + {Lpf2Hub(), HUB0_ADDR, "hub0", false, 0, 0}, + {Lpf2Hub(), HUB1_ADDR, "hub1", false, 0, 0}, +}; + +// Indexes: 0 rightTrack, 1 leftTrack, 2 bodyLift, +// 3 headTilt, 4 headTurn, 5 leftArm, 6 rightArm +static PortState gPort[7] = {{999, 0}, {999, 0}, {999, 0}, {999, 0}, + {999, 0}, {999, 0}, {999, 0}}; + +static unsigned long lastFrameAt = 0; +static bool failsafeEngaged = true; + +// ================================================================= helpers + +// Deadzone, then rescale so the remaining travel still reaches full speed. 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; + long magnitude = labs((long)raw) - DEADZONE; + long scaled = (magnitude * maxSpeed) / (512L - DEADZONE); if (scaled > maxSpeed) scaled = maxSpeed; return sign * (int)scaled; } -static void driveMotor(Lpf2Hub &hub, byte port, int speed, PortState &state) { - if (!hub.isConnected()) return; +static void driveMotor(HubLink &hl, byte port, int speed, PortState &st) { + if (!hl.hub.isConnected()) return; unsigned long now = millis(); - bool changed = (speed != state.lastSpeed); - bool stopping = (speed == 0 && state.lastSpeed != 0); + bool stopping = (speed == 0 && st.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; + // Stops always go out immediately. Everything else is rate limited twice: + // per port, and per hub - hub 1 has four motors on it, and the per-port + // limit alone lets through more than the hub will swallow. + if (!stopping) { + if (speed == st.lastSpeed) return; + if ((now - st.lastSentAt) < MOTOR_MIN_GAP_MS) return; + if ((now - hl.lastCmdAt) < HUB_MIN_GAP_MS) return; + } -#if USE_TACHO_MOTORS - hub.setTachoMotorSpeed(port, speed); -#else - hub.setBasicMotorSpeed(port, speed); +#if DEBUG_MOTORS + Serial.printf("TX %s port %u speed %d\n", hl.label, port, speed); #endif - state.lastSpeed = speed; - state.lastSentAt = now; +#if USE_TACHO_MOTORS + hl.hub.setTachoMotorSpeed(port, speed); +#else + hl.hub.setBasicMotorSpeed(port, speed); +#endif + + st.lastSpeed = speed; + st.lastSentAt = now; + hl.lastCmdAt = now; } static void stopEverything() { - driveMotor(hubTracks, PORT_A, 0, trackLeft); - driveMotor(hubTracks, PORT_B, 0, trackRight); - driveMotor(hubUpperBody, PORT_A, 0, headTurn); + driveMotor(gHubs[0], PORT_A, 0, gPort[0]); + driveMotor(gHubs[0], PORT_B, 0, gPort[1]); + driveMotor(gHubs[0], PORT_D, 0, gPort[2]); + driveMotor(gHubs[1], PORT_A, 0, gPort[3]); + driveMotor(gHubs[1], PORT_B, 0, gPort[4]); + driveMotor(gHubs[1], PORT_C, 0, gPort[5]); + driveMotor(gHubs[1], PORT_D, 0, gPort[6]); } // 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; +static void serviceHub(HubLink &hl) { + if (hl.hub.isConnected()) return; - if (hub.isConnecting()) { - hub.connectHub(); - if (hub.isConnected()) { - Serial.printf("[%s] connected\n", label); - hub.setLedColor(GREEN); + if (hl.hub.isConnecting()) { + hl.hub.connectHub(); + if (hl.hub.isConnected()) { + Serial.printf("[%s] connected (%s)\n", hl.label, hl.addr); + hl.hub.setLedColor(GREEN); } else { - Serial.printf("[%s] connect failed, retrying\n", label); - initialised = false; - retryAt = millis() + 2000; + Serial.printf("[%s] connect failed, retrying\n", hl.label); + hl.initialised = false; + hl.retryAt = millis() + RECONNECT_GAP_MS; } return; } - if (!initialised && millis() >= retryAt) { - Serial.printf("[%s] scanning for %s\n", label, address); - hub.init(std::string(address)); - initialised = true; + if (!hl.initialised && millis() >= hl.retryAt) { + Serial.printf("[%s] scanning for %s\n", hl.label, hl.addr); + hl.hub.init(std::string(hl.addr)); + hl.initialised = true; } } @@ -139,8 +203,13 @@ static uint8_t xorChecksum(const char *s, size_t len) { 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) { +struct Frame { + int lx, ly, rx, ry; + unsigned buttons, dpad; + int l2, r2; +}; + +static bool parseFrame(char *line, Frame &f) { char *star = strrchr(line, '*'); if (!star) return false; *star = '\0'; @@ -149,10 +218,46 @@ static bool parseFrame(char *line, int &lx, int &ly, int &rx, int &ry, unsigned 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; + return sscanf(line, "G,%d,%d,%d,%d,%u,%u,%d,%d", + &f.lx, &f.ly, &f.rx, &f.ry, + &f.buttons, &f.dpad, &f.l2, &f.r2) == 8; } -// -------------------------------------------------------------------- main +// Tank drive. One input per motor - nothing is mixed. +static void applyFrame(const Frame &f) { + // Both shoulders together is the panic stop. + if ((f.buttons & BTN_L1) && (f.buttons & BTN_R1)) { + stopEverything(); + return; + } + + int scale = SCALE_NORMAL; + if (f.buttons & BTN_L1) scale = SCALE_PRECISION; + if (f.buttons & BTN_R1) scale = SCALE_FULL; + + int leftTrack = stickToSpeed(-f.ly, TRACK_MAX) * scale / 100; + int rightTrack = stickToSpeed(-f.ry, TRACK_MAX) * scale / 100; + + int headTilt = (f.dpad & DPAD_U) ? HEAD_MAX : (f.dpad & DPAD_D) ? -HEAD_MAX : 0; + int headTurn = (f.dpad & DPAD_R) ? HEAD_MAX : (f.dpad & DPAD_L) ? -HEAD_MAX : 0; + + // R2 raises, L2 lowers. Both analog 0..1023, so the lift stays proportional. + int bodyLift = constrain((f.r2 - f.l2) * LIFT_MAX / 1023, -LIFT_MAX, LIFT_MAX); + + int leftArm = (f.buttons & BTN_X) ? ARM_MAX : (f.buttons & BTN_B) ? -ARM_MAX : 0; + int rightArm = (f.buttons & BTN_Y) ? ARM_MAX : (f.buttons & BTN_A) ? -ARM_MAX : 0; + + driveMotor(gHubs[0], PORT_A, rightTrack, gPort[0]); + driveMotor(gHubs[0], PORT_B, leftTrack, gPort[1]); + driveMotor(gHubs[0], PORT_D, bodyLift, gPort[2]); + driveMotor(gHubs[1], PORT_A, headTilt, gPort[3]); + driveMotor(gHubs[1], PORT_B, headTurn, gPort[4]); + driveMotor(gHubs[1], PORT_C, leftArm, gPort[5]); + driveMotor(gHubs[1], PORT_D, rightArm, gPort[6]); +} + +// ==================================================================== main + void setup() { Serial.begin(115200); Serial2.begin(LINK_BAUD, SERIAL_8N1, LINK_RX_PIN, LINK_TX_PIN); @@ -164,17 +269,15 @@ void setup() { } 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"); - } + // 1. Keep the hubs connected, hub0 first. + serviceHub(gHubs[0]); + if (gHubs[0].hub.isConnected()) serviceHub(gHubs[1]); - digitalWrite(STATUS_LED_PIN, - (hubTracks.isConnected() && hubUpperBody.isConnected()) ? HIGH : LOW); + bool ready = gHubs[0].hub.isConnected() && gHubs[1].hub.isConnected(); + digitalWrite(STATUS_LED_PIN, ready ? HIGH : LOW); // 2. Pull whole lines off the link. - static char buf[96]; + static char buf[128]; static size_t idx = 0; while (Serial2.available()) { @@ -182,19 +285,11 @@ void loop() { 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)) { + Frame f; + if (idx > 0 && parseFrame(buf, f)) { 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); + applyFrame(f); } idx = 0; } else if (idx < sizeof(buf) - 1) {