From f5e7517cbda9b6337486cac127a282e5b6b20eed Mon Sep 17 00:00:00 2001 From: jessikitty Date: Thu, 10 Sep 2026 16:20:44 +1000 Subject: [PATCH] Evolved control scheme - Correct hub1 port map: A is the left arm, C is the head tilt. - Arms: L2/R2 raise proportionally, L3/R3 lower at reduced power. - Head turn onto L1/R1, head tilt onto d-pad left/right, body lift onto d-pad up/down. All move while held. - Square is now the panic stop; Cross toggles 50/100% track speed. - Circle and Triangle toggle two GPIO LED circuits on pins 25 and 26. - Latching controls are edge-detected; frames arrive at ~50Hz so a held button would otherwise toggle continuously. --- receiver/receiver.ino | 147 ++++++++++++++++++++++++++++-------------- 1 file changed, 100 insertions(+), 47 deletions(-) diff --git a/receiver/receiver.ino b/receiver/receiver.ino index 91e3ea7..4f9e2d4 100644 --- a/receiver/receiver.ino +++ b/receiver/receiver.ino @@ -1,7 +1,8 @@ /* * receiver.ino -- UART -> two LEGO Powered Up hubs (ESP32 "B") * - * Model: Johnny 5 (Short Circuit) MOC, 7 motors across 2 Technic hubs. + * 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) @@ -9,17 +10,16 @@ * 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. * - * Listens for gamepad frames from the Bluepad32 board on Serial2 and drives - * 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 (mandatory - common ground) * - * Control scheme is tank drive: every input drives exactly one motor. + * LED circuits (see LED_1_PIN / LED_2_PIN below): + * 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. * * FILE ORDER MATTERS: * The Arduino IDE injects generated function prototypes immediately before @@ -38,10 +38,10 @@ // 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 head tilt +// A right track A left arm // B left track B head turn -// D body lift C left arm -// D right arm +// C (free) C head tilt +// 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"; @@ -51,8 +51,12 @@ 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 + // 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 // Set to 1 to log every motor command that goes out. Useful for proving which @@ -61,9 +65,7 @@ static const byte PORT_D = 0x03; static const int DEADZONE = 40; // raw stick counts ignored around centre -// Motor directions. Flip to -1 if an axis runs backwards - mirrored mountings -// are normal on a symmetric model, and this is cheaper than editing signs -// scattered through applyFrame(). +// 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; @@ -75,15 +77,15 @@ static const int DIR_RIGHT_ARM = 1; // 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; +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_UP_MAX = 45; +static const int ARM_DOWN_MAX = 30; // gravity helps on the way down -// 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; +// Track speed, toggled by Cross. +static const int SPEED_SLOW = 50; +static const int SPEED_FAST = 100; // Bluepad32 button masks. Verify against your own pad with DEBUG_BUTTONS in // the transmitter sketch if any of these seem wrong. @@ -93,12 +95,17 @@ 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 BTN_L3 = 0x0100; // left stick click +static const unsigned BTN_R3 = 0x0200; // right stick click 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 @@ -139,14 +146,21 @@ static HubLink gHubs[2] = { {Lpf2Hub(), HUB1_ADDR, "hub1", false, 0, 0}, }; -// Indexes: 0 rightTrack, 1 leftTrack, 2 bodyLift, -// 3 headTilt, 4 headTurn, 5 leftArm, 6 rightArm +// Indexes follow physical ports, not functions: +// 0 hub0/A 1 hub0/B 2 hub0/D +// 3 hub1/A 4 hub1/B 5 hub1/C 6 hub1/D 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; +// 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. @@ -159,6 +173,15 @@ static int stickToSpeed(int raw, int maxSpeed) { return sign * (int)scaled; } +// Analog trigger, 0..1023, to a 0..maxSpeed power with a deadzone at the bottom. +static int triggerToSpeed(int raw, int maxSpeed) { + if (raw <= TRIGGER_DEADZONE) return 0; + long scaled = ((long)(raw - TRIGGER_DEADZONE) * maxSpeed) / + (1023L - TRIGGER_DEADZONE); + if (scaled > maxSpeed) scaled = maxSpeed; + return (int)scaled; +} + static void driveMotor(HubLink &hl, byte port, int speed, PortState &st) { if (!hl.hub.isConnected()) return; @@ -244,43 +267,68 @@ static bool parseFrame(char *line, Frame &f) { &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) { - // Both shoulders together is the panic stop. - if ((f.buttons & BTN_L1) && (f.buttons & BTN_R1)) { + handleLatchingButtons(f.buttons); + + // Square is the panic stop. LEDs are left alone - they are not motion. + if (f.buttons & BTN_X) { 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) * gTrackSpeed / 100 + * DIR_LEFT_TRACK; + int rightTrack = stickToSpeed(-f.ry, TRACK_MAX) * gTrackSpeed / 100 + * DIR_RIGHT_TRACK; - int leftTrack = stickToSpeed(-f.ly, TRACK_MAX) * scale / 100 * DIR_LEFT_TRACK; - int rightTrack = stickToSpeed(-f.ry, TRACK_MAX) * scale / 100 * DIR_RIGHT_TRACK; + // L1 / R1 swing the head while held. + int headTurn = ((f.buttons & BTN_R1) ? HEAD_MAX + : (f.buttons & BTN_L1) ? -HEAD_MAX : 0) * DIR_HEAD_TURN; - int headTilt = ((f.dpad & DPAD_U) ? HEAD_MAX - : (f.dpad & DPAD_D) ? -HEAD_MAX : 0) * DIR_HEAD_TILT; - int headTurn = ((f.dpad & DPAD_R) ? HEAD_MAX - : (f.dpad & DPAD_L) ? -HEAD_MAX : 0) * DIR_HEAD_TURN; + // D-pad: up/down lifts the body, left/right tilts the head. Both move + // while held. + 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; - // 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) * DIR_BODY_LIFT; + // Arms: analog trigger raises proportionally, stick click lowers. + int leftRaise = triggerToSpeed(f.l2, ARM_UP_MAX); + int rightRaise = triggerToSpeed(f.r2, ARM_UP_MAX); - // Square raises the left arm, Circle lowers it. - int leftArm = ((f.buttons & BTN_X) ? ARM_MAX - : (f.buttons & BTN_B) ? -ARM_MAX : 0) * DIR_LEFT_ARM; - // Cross raises the right arm, Triangle lowers it. - int rightArm = ((f.buttons & BTN_A) ? ARM_MAX - : (f.buttons & BTN_Y) ? -ARM_MAX : 0) * DIR_RIGHT_ARM; + int leftArm = ((f.buttons & BTN_L3) ? -ARM_DOWN_MAX : leftRaise) + * DIR_LEFT_ARM; + int rightArm = ((f.buttons & BTN_R3) ? -ARM_DOWN_MAX : rightRaise) + * DIR_RIGHT_ARM; 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_A, leftArm, gPort[3]); driveMotor(gHubs[1], PORT_B, headTurn, gPort[4]); - driveMotor(gHubs[1], PORT_C, leftArm, gPort[5]); + driveMotor(gHubs[1], PORT_C, headTilt, gPort[5]); driveMotor(gHubs[1], PORT_D, rightArm, gPort[6]); } @@ -293,7 +341,12 @@ void setup() { pinMode(STATUS_LED_PIN, OUTPUT); digitalWrite(STATUS_LED_PIN, LOW); - Serial.println("LEGO hub receiver starting"); + 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)"); } void loop() {