/* * receiver.ino -- UART -> two LEGO Powered Up hubs (ESP32 "B") * * Model: Johnny 5 (Short Circuit) MOC - "Evolved" control scheme. * 7 motors across 2 Technic hubs, plus 2 GPIO-driven LED circuits. * * Board package: esp32 (the normal Espressif one), core 2.0.17 * Libraries: Legoino + NimBLE-Arduino 1.4.x (both via Library Manager) * * Core 3.x will not build Legoino - you get 'std::string does not name a type' * and a ReadUInt32LE declaration mismatch. Stay on 2.0.17 for this board. * Do NOT select the esp32_bluepad32 package here either: it starts BTstack * before setup() runs and NimBLE aborts with ESP_ERR_INVALID_STATE. * * Wiring to the transmitter board: * RX GPIO16 <- TX GPIO17 on transmitter * TX GPIO17 -> RX GPIO16 on transmitter * GND -> GND (mandatory - common ground) * * LED circuits - both on THIS board, returning to THIS board's GND: * GPIO25 -> resistor -> LED pair -> GND * GPIO26 -> resistor -> LED pair -> GND * Pins output 3.3V, not 3V. ~12mA per pin is comfortable, 40mA is the hard * limit. Anything drawing more than ~20mA per circuit needs a transistor. * Do not tap the LED return off the UART ground wire to the other board - * that reference needs to stay clean. * * STARTUP ORDER: * Wake BOTH hubs with their green buttons and check both are blinking, * THEN power this board. hub1 is only serviced once hub0 is connected, so a * sleeping hub0 blocks the whole sequence. Hubs stop advertising after a * couple of minutes idle. * * ARMS ARE POSITION CONTROLLED: * The triggers set an ANGLE, not a power level. Trigger released holds the * arm at 0, fully depressed holds it at ARM_SPAN_*. The encoders are zeroed * once, on the first hub1 connect after boot, so BOTH ARMS MUST BE DOWN at * that moment. They are deliberately NOT re-zeroed on a reconnect - the hub * keeps its encoder preset, and re-zeroing mid-session would redefine zero * at whatever position the arms happened to be in. * * FILE ORDER MATTERS: * The Arduino IDE injects generated function prototypes immediately before * the FIRST function definition in the file. Any type used in a function * signature must be declared above that point - which is why HubLink, * PortState, ArmState and Frame all live in the types block. * * 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. // // hub 0 - lower body hub 1 - upper body // A right track A left arm // B left track B head tilt // C (free) C head turn // D body lift D right arm static const char *HUB0_ADDR = "90:84:2b:61:f2:d7"; static const char *HUB1_ADDR = "90:84:2b:61:e6:8c"; static const byte PORT_A = 0x00; static const byte PORT_B = 0x01; static const byte PORT_C = 0x02; static const byte PORT_D = 0x03; // LED circuits. Safe GPIOs - no boot strapping or flash duties, unlike 0, 2, // 12 and 15. static const int LED_1_PIN = 25; // Circle toggles this pair static const int LED_2_PIN = 26; // Triangle toggles this pair #define USE_TACHO_MOTORS 1 // Set to 1 to log every command that goes out. Noisy - turn it back off. #define DEBUG_MOTORS 0 static const int DEADZONE = 40; // raw stick counts ignored around centre // Motor directions. Flip to -1 if an axis runs backwards. static const int DIR_LEFT_TRACK = 1; static const int DIR_RIGHT_TRACK = -1; static const int DIR_BODY_LIFT = 1; static const int DIR_HEAD_TILT = 1; static const int DIR_HEAD_TURN = 1; // Arm travel in MOTOR degrees, measured with tools/arm_calibrate. // Raw measurements were roughly +280 (left) and -275 (right). These are set // slightly short so backlash cannot stall the motor against the top stop. // The sign carries the direction - there is no DIR_ constant for the arms. static const int32_t ARM_SPAN_LEFT = 250; static const int32_t ARM_SPAN_RIGHT = -250; static const int ARM_SPEED = 60; // how fast it travels to the target static const byte ARM_MAX_POWER = 40; // torque cap - keeps a jam survivable static const int ARM_STEP_DEG = 3; // ignore target changes smaller than this // Arms get their own command interval. They are position controlled, so the // target moves continuously with the trigger and needs updating more often // than a velocity axis does - at the per-port 100ms the arm sprints to each // target then sits idle, which feels like stepping. static const unsigned long ARM_MIN_GAP_MS = 60; // Per-axis power caps for the velocity-controlled axes. static const int TRACK_MAX = 100; // LEGO speed range is -100..100 static const int HEAD_MAX = 45; static const int LIFT_MAX = 60; // Track speed, toggled by Cross. static const int SPEED_SLOW = 50; static const int SPEED_FAST = 100; // Bluepad32 button masks. Verify with DEBUG_BUTTONS in the transmitter. 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; // Triggers rest at 0 and are noisy near the bottom of their travel. static const int TRIGGER_DEADZONE = 60; 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; // A Legoino scan that never finds its hub expires silently, leaving the hub // stuck waiting forever. This is how long we give it before starting over. static const unsigned long SCAN_TIMEOUT_MS = 12000; 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; // =================================================================== types struct HubLink { Lpf2Hub hub; const char *addr; const char *label; bool initialised; unsigned long retryAt; unsigned long lastCmdAt; // per-hub rate limit, shared across ports unsigned long scanExpiresAt; // when to give up on the current scan }; struct PortState { int lastSpeed; unsigned long lastSentAt; }; struct ArmState { int32_t lastTarget; unsigned long lastSentAt; }; struct Frame { int lx, ly, rx, ry; unsigned buttons, dpad; int l2, r2; }; // ================================================================= globals static HubLink gHubs[2] = { {Lpf2Hub(), HUB0_ADDR, "hub0", false, 0, 0, 0}, {Lpf2Hub(), HUB1_ADDR, "hub1", false, 0, 0, 0}, }; // Indexes follow physical ports, not functions: // 0 hub0/A 1 hub0/B 2 hub0/D 3 hub1/B 4 hub1/C static PortState gPort[5] = {{999, 0}, {999, 0}, {999, 0}, {999, 0}, {999, 0}}; // Arms are position controlled, so they get their own state. static ArmState gArmLeft = {INT32_MIN, 0}; static ArmState gArmRight = {INT32_MIN, 0}; static bool gArmsZeroed = false; static unsigned long lastFrameAt = 0; static bool failsafeEngaged = true; // Latched state, changed on button press rather than while held. static int gTrackSpeed = SPEED_SLOW; static bool gLed1On = false; static bool gLed2On = false; static unsigned gPrevButtons = 0; // ================================================================= 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 = labs((long)raw) - DEADZONE; long scaled = (magnitude * maxSpeed) / (512L - DEADZONE); if (scaled > maxSpeed) scaled = maxSpeed; return sign * (int)scaled; } // Analog trigger, 0..1023, to a target angle between 0 and span. static int32_t triggerToAngle(int raw, int32_t span) { if (raw <= TRIGGER_DEADZONE) return 0; long travel = (long)raw - TRIGGER_DEADZONE; long full = 1023L - TRIGGER_DEADZONE; if (travel > full) travel = full; return (int32_t)((travel * span) / full); } static void driveMotor(HubLink &hl, byte port, int speed, PortState &st) { if (!hl.hub.isConnected()) return; unsigned long now = millis(); bool stopping = (speed == 0 && st.lastSpeed != 0); // Stops always go out immediately. Everything else is rate limited twice: // per port, and per hub - the per-port limit alone lets through more than // a hub with several motors on it 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 DEBUG_MOTORS Serial.printf("TX %s port %u speed %d\n", hl.label, port, speed); #endif #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; } // Position control. HOLD keeps the motor actively at the target rather than // letting gravity drag the arm back down. static void driveArm(HubLink &hl, byte port, int32_t target, ArmState &st) { if (!hl.hub.isConnected() || !gArmsZeroed) return; unsigned long now = millis(); if (labs((long)target - (long)st.lastTarget) < ARM_STEP_DEG) return; if ((now - st.lastSentAt) < ARM_MIN_GAP_MS) return; if ((now - hl.lastCmdAt) < HUB_MIN_GAP_MS) return; #if DEBUG_MOTORS Serial.printf("TX %s port %u angle %ld\n", hl.label, port, (long)target); #endif hl.hub.setAbsoluteMotorPosition(port, ARM_SPEED, target, ARM_MAX_POWER, BrakingStyle::HOLD); st.lastTarget = target; st.lastSentAt = now; hl.lastCmdAt = now; } static void stopEverything() { 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_B, 0, gPort[3]); driveMotor(gHubs[1], PORT_C, 0, gPort[4]); // Arms: a plain speed command overrides the position hold and goes limp. // Reset the cached targets so the next trigger movement re-commands. if (gHubs[1].hub.isConnected()) { gHubs[1].hub.setTachoMotorSpeed(PORT_A, 0); gHubs[1].hub.setTachoMotorSpeed(PORT_D, 0); } gArmLeft.lastTarget = INT32_MIN; gArmRight.lastTarget = INT32_MIN; } // 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. // // Note the 'initialised' one-shot. init() starts an ASYNCHRONOUS scan, so // immediately afterwards isConnected() and isConnecting() are both still // false. Guarding on those alone re-enters NimBLEDevice::init() thousands of // times a second and the Bluetooth controller aborts. // // The scanExpiresAt deadline exists because a scan that finds nothing just // ends quietly - isConnecting() never goes true, so without a timeout the hub // sits on a dead scan until the board is power cycled. static void serviceHub(HubLink &hl) { if (hl.hub.isConnected()) return; 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", hl.label); hl.initialised = false; hl.retryAt = millis() + RECONNECT_GAP_MS; } return; } if (hl.initialised && millis() >= hl.scanExpiresAt) { Serial.printf("[%s] scan timed out, restarting\n", hl.label); hl.initialised = false; hl.retryAt = millis() + RECONNECT_GAP_MS; return; } 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; hl.scanExpiresAt = millis() + SCAN_TIMEOUT_MS; } } // Define "arms down" as angle zero. Runs once per boot, after hub1 connects, // with the arms physically at the bottom of their travel. Not repeated on a // reconnect: the hub keeps its encoder preset, and re-zeroing mid-session // would redefine zero wherever the arms happened to be sitting. static void zeroArms() { if (gArmsZeroed || !gHubs[1].hub.isConnected()) return; delay(500); // let the hub finish reporting its ports gHubs[1].hub.setAbsoluteMotorEncoderPosition(PORT_A, 0); delay(200); gHubs[1].hub.setAbsoluteMotorEncoderPosition(PORT_D, 0); delay(200); gArmsZeroed = true; gArmLeft.lastTarget = INT32_MIN; gArmRight.lastTarget = INT32_MIN; Serial.println("Arms zeroed at current position"); } 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; } static bool parseFrame(char *line, Frame &f) { 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,%u,%d,%d", &f.lx, &f.ly, &f.rx, &f.ry, &f.buttons, &f.dpad, &f.l2, &f.r2) == 8; } // Latching controls fire once per press, not continuously while held. Frames // arrive at ~50 Hz, so without edge detection a single press would toggle // twenty times. static void handleLatchingButtons(unsigned buttons) { unsigned pressed = buttons & ~gPrevButtons; gPrevButtons = buttons; if (pressed & BTN_A) { // Cross - alternate track speed gTrackSpeed = (gTrackSpeed == SPEED_FAST) ? SPEED_SLOW : SPEED_FAST; Serial.printf("track speed %d%%\n", gTrackSpeed); } if (pressed & BTN_B) { // Circle - LED pair 1 gLed1On = !gLed1On; digitalWrite(LED_1_PIN, gLed1On ? HIGH : LOW); } if (pressed & BTN_Y) { // Triangle - LED pair 2 gLed2On = !gLed2On; digitalWrite(LED_2_PIN, gLed2On ? HIGH : LOW); } } // Tank drive. One input per motor - nothing is mixed. static void applyFrame(const Frame &f) { handleLatchingButtons(f.buttons); // Square is the panic stop. LEDs are left alone - they are not motion. if (f.buttons & BTN_X) { stopEverything(); return; } int leftTrack = stickToSpeed(-f.ly, TRACK_MAX) * gTrackSpeed / 100 * DIR_LEFT_TRACK; int rightTrack = stickToSpeed(-f.ry, TRACK_MAX) * gTrackSpeed / 100 * DIR_RIGHT_TRACK; // L1 / R1 turn the head while held. int headTurn = ((f.buttons & BTN_R1) ? HEAD_MAX : (f.buttons & BTN_L1) ? -HEAD_MAX : 0) * DIR_HEAD_TURN; // D-pad: up/down lifts the body, left/right tilts the head. int bodyLift = ((f.dpad & DPAD_U) ? LIFT_MAX : (f.dpad & DPAD_D) ? -LIFT_MAX : 0) * DIR_BODY_LIFT; int headTilt = ((f.dpad & DPAD_R) ? HEAD_MAX : (f.dpad & DPAD_L) ? -HEAD_MAX : 0) * DIR_HEAD_TILT; // Arms: trigger position IS arm angle. Released means "go to zero", which // gravity is already doing, so the motor mostly just catches it. int32_t leftTarget = triggerToAngle(f.l2, ARM_SPAN_LEFT); int32_t rightTarget = triggerToAngle(f.r2, ARM_SPAN_RIGHT); driveMotor(gHubs[0], PORT_A, rightTrack, gPort[0]); driveMotor(gHubs[0], PORT_B, leftTrack, gPort[1]); driveMotor(gHubs[0], PORT_D, bodyLift, gPort[2]); // hub1 B is the tilt motor and C is the turn motor - the reverse of what // the first build assumed. driveMotor(gHubs[1], PORT_B, headTilt, gPort[3]); driveMotor(gHubs[1], PORT_C, headTurn, gPort[4]); driveArm(gHubs[1], PORT_A, leftTarget, gArmLeft); driveArm(gHubs[1], PORT_D, rightTarget, gArmRight); } // ==================================================================== 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); pinMode(LED_1_PIN, OUTPUT); pinMode(LED_2_PIN, OUTPUT); digitalWrite(LED_1_PIN, LOW); digitalWrite(LED_2_PIN, LOW); Serial.println("LEGO hub receiver starting (Johnny 5 Evolved)"); Serial.println("Wake both hubs first. Both arms must be DOWN."); } void loop() { // 1. Keep the hubs connected, hub0 first. serviceHub(gHubs[0]); if (gHubs[0].hub.isConnected()) serviceHub(gHubs[1]); // Zeroes once per boot. Deliberately not reset when the hub drops. if (gHubs[1].hub.isConnected()) zeroArms(); 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[128]; static size_t idx = 0; while (Serial2.available()) { char c = (char)Serial2.read(); if (c == '\r') continue; if (c == '\n') { buf[idx] = '\0'; Frame f; if (idx > 0 && parseFrame(buf, f)) { lastFrameAt = millis(); failsafeEngaged = false; applyFrame(f); } 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; } }