jessikitty/ps4-lego-bridge
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Evolved control scheme

Browse Source 
- 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.
This commit is contained in:
jessikitty
2026-09-10 16:20:44 +10:00
parent 8fac5c893e
commit f5e7517cbd
1 changed files with 100 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
receiver/receiver.ino
+100 -47
View File
@@ -1,7 +1,8 @@
/* /*
* receiver.ino -- UART -> two LEGO Powered Up hubs (ESP32 "B") * 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 * Board package: esp32 (the normal Espressif one), core 2.0.17
* Libraries: Legoino + NimBLE-Arduino 1.4.x (both via Library Manager) * 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' * 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. * 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: * Wiring to the transmitter board:
* RX GPIO16 <- TX GPIO17 on transmitter * RX GPIO16 <- TX GPIO17 on transmitter
* TX GPIO17 -> RX GPIO16 on transmitter * TX GPIO17 -> RX GPIO16 on transmitter
* GND -> GND (mandatory - common ground) * 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: * FILE ORDER MATTERS:
* The Arduino IDE injects generated function prototypes immediately before * 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 BLE addresses. Run tools/hub_scanner to find them - do not guess.
// //
// hub 0 - lower body hub 1 - upper body // 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 // B left track B head turn
// D body lift C left arm // C (free) C head tilt
// D right arm // D body lift D right arm
static const char *HUB0_ADDR = "90:84:2b:61:f2:d7"; static const char *HUB0_ADDR = "90:84:2b:61:f2:d7";
static const char *HUB1_ADDR = "90:84:2b:61:e6:8c"; 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_C = 0x02;
static const byte PORT_D = 0x03; 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. // 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 #define USE_TACHO_MOTORS 1
// Set to 1 to log every motor command that goes out. Useful for proving which // 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 static const int DEADZONE = 40; // raw stick counts ignored around centre
// Motor directions. Flip to -1 if an axis runs backwards - mirrored mountings // Motor directions. Flip to -1 if an axis runs backwards.
// are normal on a symmetric model, and this is cheaper than editing signs
// scattered through applyFrame().
static const int DIR_LEFT_TRACK = 1; static const int DIR_LEFT_TRACK = 1;
static const int DIR_RIGHT_TRACK = -1; static const int DIR_RIGHT_TRACK = -1;
static const int DIR_BODY_LIFT = 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 // 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 // stop and there is no position feedback, so holding a direction at a stop
// stalls the motor. Lower these if an axis feels forceful. // 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 TRACK_MAX = 100; // LEGO speed range is -100..100
static const int HEAD_MAX = 45; static const int HEAD_MAX = 45;
static const int LIFT_MAX = 60; static const int LIFT_MAX = 60;
static const int ARM_MAX = 45; 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). // Track speed, toggled by Cross.
static const int SCALE_NORMAL = 75; static const int SPEED_SLOW = 50;
static const int SCALE_PRECISION = 40; static const int SPEED_FAST = 100;
static const int SCALE_FULL = 100;
// Bluepad32 button masks. Verify against your own pad with DEBUG_BUTTONS in // Bluepad32 button masks. Verify against your own pad with DEBUG_BUTTONS in
// the transmitter sketch if any of these seem wrong. // 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_Y = 0x0008; // Triangle
static const unsigned BTN_L1 = 0x0010; static const unsigned BTN_L1 = 0x0010;
static const unsigned BTN_R1 = 0x0020; 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_U = 0x01;
static const unsigned DPAD_D = 0x02; static const unsigned DPAD_D = 0x02;
static const unsigned DPAD_R = 0x04; static const unsigned DPAD_R = 0x04;
static const unsigned DPAD_L = 0x08; 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 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 HUB_MIN_GAP_MS = 25; // per hub, ~40 cmd/sec
static const unsigned long LINK_TIMEOUT_MS = 400; // failsafe static const unsigned long LINK_TIMEOUT_MS = 400; // failsafe
@@ -139,14 +146,21 @@ static HubLink gHubs[2] = {
{Lpf2Hub(), HUB1_ADDR, "hub1", false, 0, 0}, {Lpf2Hub(), HUB1_ADDR, "hub1", false, 0, 0},
}; };
// Indexes: 0 rightTrack, 1 leftTrack, 2 bodyLift, // Indexes follow physical ports, not functions:
// 3 headTilt, 4 headTurn, 5 leftArm, 6 rightArm // 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}, static PortState gPort[7] = {{999, 0}, {999, 0}, {999, 0}, {999, 0},
{999, 0}, {999, 0}, {999, 0}}; {999, 0}, {999, 0}, {999, 0}};
static unsigned long lastFrameAt = 0; static unsigned long lastFrameAt = 0;
static bool failsafeEngaged = true; 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 // ================================================================= helpers
// Deadzone, then rescale so the remaining travel still reaches full speed. // 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; 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) { static void driveMotor(HubLink &hl, byte port, int speed, PortState &st) {
if (!hl.hub.isConnected()) return; 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; &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. // Tank drive. One input per motor - nothing is mixed.
static void applyFrame(const Frame &f) { static void applyFrame(const Frame &f) {
// Both shoulders together is the panic stop. handleLatchingButtons(f.buttons);
if ((f.buttons & BTN_L1) && (f.buttons & BTN_R1)) {
// Square is the panic stop. LEDs are left alone - they are not motion.
if (f.buttons & BTN_X) {
stopEverything(); stopEverything();
return; return;
} }
int scale = SCALE_NORMAL; int leftTrack = stickToSpeed(-f.ly, TRACK_MAX) * gTrackSpeed / 100
if (f.buttons & BTN_L1) scale = SCALE_PRECISION; * DIR_LEFT_TRACK;
if (f.buttons & BTN_R1) scale = SCALE_FULL; int rightTrack = stickToSpeed(-f.ry, TRACK_MAX) * gTrackSpeed / 100
* DIR_RIGHT_TRACK;
int leftTrack = stickToSpeed(-f.ly, TRACK_MAX) * scale / 100 * DIR_LEFT_TRACK; // L1 / R1 swing the head while held.
int rightTrack = stickToSpeed(-f.ry, TRACK_MAX) * scale / 100 * DIR_RIGHT_TRACK; 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 // D-pad: up/down lifts the body, left/right tilts the head. Both move
: (f.dpad & DPAD_D) ? -HEAD_MAX : 0) * DIR_HEAD_TILT; // while held.
int headTurn = ((f.dpad & DPAD_R) ? HEAD_MAX int bodyLift = ((f.dpad & DPAD_U) ? LIFT_MAX
: (f.dpad & DPAD_L) ? -HEAD_MAX : 0) * DIR_HEAD_TURN; : (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. // Arms: analog trigger raises proportionally, stick click lowers.
int bodyLift = constrain((f.r2 - f.l2) * LIFT_MAX / 1023, int leftRaise = triggerToSpeed(f.l2, ARM_UP_MAX);
-LIFT_MAX, LIFT_MAX) * DIR_BODY_LIFT; int rightRaise = triggerToSpeed(f.r2, ARM_UP_MAX);
// Square raises the left arm, Circle lowers it. int leftArm = ((f.buttons & BTN_L3) ? -ARM_DOWN_MAX : leftRaise)
int leftArm = ((f.buttons & BTN_X) ? ARM_MAX * DIR_LEFT_ARM;
: (f.buttons & BTN_B) ? -ARM_MAX : 0) * DIR_LEFT_ARM; int rightArm = ((f.buttons & BTN_R3) ? -ARM_DOWN_MAX : rightRaise)
// Cross raises the right arm, Triangle lowers it. * DIR_RIGHT_ARM;
int rightArm = ((f.buttons & BTN_A) ? ARM_MAX
: (f.buttons & BTN_Y) ? -ARM_MAX : 0) * DIR_RIGHT_ARM;
driveMotor(gHubs[0], PORT_A, rightTrack, gPort[0]); driveMotor(gHubs[0], PORT_A, rightTrack, gPort[0]);
driveMotor(gHubs[0], PORT_B, leftTrack, gPort[1]); driveMotor(gHubs[0], PORT_B, leftTrack, gPort[1]);
driveMotor(gHubs[0], PORT_D, bodyLift, gPort[2]); 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_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]); driveMotor(gHubs[1], PORT_D, rightArm, gPort[6]);
} }
@@ -293,7 +341,12 @@ void setup() {
pinMode(STATUS_LED_PIN, OUTPUT); pinMode(STATUS_LED_PIN, OUTPUT);
digitalWrite(STATUS_LED_PIN, LOW); 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() { void loop() {