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

Rewrite receiver for all 7 Johnny 5 motors with tank drive

Browse Source 
- 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.
This commit is contained in:
jessikitty
2026-09-09 19:29:12 +10:00
parent 6622dbd05c
commit 0b32384578
1 changed files with 181 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
receiver/receiver.ino
+181 -86
View File
@@ -1,16 +1,22 @@
/* /*
* 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.
*
* Board package: esp32 (the normal Espressif one) * Board package: esp32 (the normal Espressif one)
* Libraries: Legoino + NimBLE-Arduino 1.4.x (both via Library Manager) * Libraries: Legoino + NimBLE-Arduino 1.4.x (both via Library Manager)
* *
* Listens for gamepad frames from the Bluepad32 board on Serial2 and drives * 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: * 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 * 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) * Created by: Jess Rogerson (yelling commands at Claude.AI)
*/ */
@@ -20,116 +26,174 @@
// ---------------------------------------------------------------- settings // ---------------------------------------------------------------- settings
// 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.
// Lower case, colon separated. //
static const char *HUB_TRACKS_ADDR = "90:84:2b:61:e6:8c"; // hub 0 - lower body hub 1 - upper body
static const char *HUB_UPPERBODY_ADDR = "90:84:2b:61:f2:d7"; // 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_A = 0x00;
static const byte PORT_B = 0x01; 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;
// 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. // Plain train motors and the simple Powered Up motors -> set it to 0.
#define USE_TACHO_MOTORS 1 #define USE_TACHO_MOTORS 1
static const int DEADZONE = 40; // raw stick counts ignored around centre // Set to 1 to log every motor command that goes out. Useful for proving which
static const int TRACK_MAX = 100; // LEGO speed range is -100..100 // port a command actually lands on. Noisy - turn it back off afterwards.
static const int HEAD_MAX = 60; #define DEBUG_MOTORS 0
static const unsigned long MOTOR_MIN_INTERVAL_MS = 60; // per port throttle
static const unsigned long LINK_TIMEOUT_MS = 400; // failsafe 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_RX_PIN = 16;
static const int LINK_TX_PIN = 17; static const int LINK_TX_PIN = 17;
static const long LINK_BAUD = 115200; static const long LINK_BAUD = 115200;
static const int STATUS_LED_PIN = 2; static const int STATUS_LED_PIN = 2;
// ------------------------------------------------------------------ state // =================================================================== types
Lpf2Hub hubTracks;
Lpf2Hub hubUpperBody;
static bool tracksInitialised = false; struct HubLink {
static bool upperInitialised = false; Lpf2Hub hub;
static unsigned long tracksRetryAt = 0; const char *addr;
static unsigned long upperRetryAt = 0; const char *label;
bool initialised;
static unsigned long lastFrameAt = 0; unsigned long retryAt;
static bool failsafeEngaged = true; unsigned long lastCmdAt; // per-hub rate limit, shared across its ports
};
struct PortState { struct PortState {
int lastSpeed; int lastSpeed;
unsigned long lastSentAt; 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 static HubLink gHubs[2] = {
// reaches full speed instead of jumping from 0 to a third of the range. {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) { static int stickToSpeed(int raw, int maxSpeed) {
if (raw > -DEADZONE && raw < DEADZONE) return 0; if (raw > -DEADZONE && raw < DEADZONE) return 0;
int sign = (raw < 0) ? -1 : 1; int sign = (raw < 0) ? -1 : 1;
long magnitude = abs((long)raw) - DEADZONE; long magnitude = labs((long)raw) - DEADZONE;
long span = 512L - DEADZONE; long scaled = (magnitude * maxSpeed) / (512L - DEADZONE);
long scaled = (magnitude * maxSpeed) / span;
if (scaled > maxSpeed) scaled = maxSpeed; if (scaled > maxSpeed) scaled = maxSpeed;
return sign * (int)scaled; return sign * (int)scaled;
} }
static void driveMotor(Lpf2Hub &hub, byte port, int speed, PortState &state) { static void driveMotor(HubLink &hl, byte port, int speed, PortState &st) {
if (!hub.isConnected()) return; if (!hl.hub.isConnected()) return;
unsigned long now = millis(); unsigned long now = millis();
bool changed = (speed != state.lastSpeed); bool stopping = (speed == 0 && st.lastSpeed != 0);
bool stopping = (speed == 0 && state.lastSpeed != 0);
// Stops always go out immediately; everything else is throttled so we // Stops always go out immediately. Everything else is rate limited twice:
// don't flood the hub's BLE queue and stall it. // per port, and per hub - hub 1 has four motors on it, and the per-port
if (!stopping && (!changed || (now - state.lastSentAt) < MOTOR_MIN_INTERVAL_MS)) return; // 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 #if DEBUG_MOTORS
hub.setTachoMotorSpeed(port, speed); Serial.printf("TX %s port %u speed %d\n", hl.label, port, speed);
#else
hub.setBasicMotorSpeed(port, speed);
#endif #endif
state.lastSpeed = speed; #if USE_TACHO_MOTORS
state.lastSentAt = now; hl.hub.setTachoMotorSpeed(port, speed);
#else
hl.hub.setBasicMotorSpeed(port, speed);
#endif
st.lastSpeed = speed;
st.lastSentAt = now;
hl.lastCmdAt = now;
} }
static void stopEverything() { static void stopEverything() {
driveMotor(hubTracks, PORT_A, 0, trackLeft); driveMotor(gHubs[0], PORT_A, 0, gPort[0]);
driveMotor(hubTracks, PORT_B, 0, trackRight); driveMotor(gHubs[0], PORT_B, 0, gPort[1]);
driveMotor(hubUpperBody, PORT_A, 0, headTurn); 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 // 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. // shared NimBLE scanner and you end up with one hub connected and one sulking.
static void serviceHub(Lpf2Hub &hub, const char *address, bool &initialised, static void serviceHub(HubLink &hl) {
unsigned long &retryAt, const char *label) { if (hl.hub.isConnected()) return;
if (hub.isConnected()) return;
if (hub.isConnecting()) { if (hl.hub.isConnecting()) {
hub.connectHub(); hl.hub.connectHub();
if (hub.isConnected()) { if (hl.hub.isConnected()) {
Serial.printf("[%s] connected\n", label); Serial.printf("[%s] connected (%s)\n", hl.label, hl.addr);
hub.setLedColor(GREEN); hl.hub.setLedColor(GREEN);
} else { } else {
Serial.printf("[%s] connect failed, retrying\n", label); Serial.printf("[%s] connect failed, retrying\n", hl.label);
initialised = false; hl.initialised = false;
retryAt = millis() + 2000; hl.retryAt = millis() + RECONNECT_GAP_MS;
} }
return; return;
} }
if (!initialised && millis() >= retryAt) { if (!hl.initialised && millis() >= hl.retryAt) {
Serial.printf("[%s] scanning for %s\n", label, address); Serial.printf("[%s] scanning for %s\n", hl.label, hl.addr);
hub.init(std::string(address)); hl.hub.init(std::string(hl.addr));
initialised = true; hl.initialised = true;
} }
} }
@@ -139,8 +203,13 @@ static uint8_t xorChecksum(const char *s, size_t len) {
return c; return c;
} }
// Returns true if a valid frame was parsed. struct Frame {
static bool parseFrame(char *line, int &lx, int &ly, int &rx, int &ry, unsigned &buttons) { int lx, ly, rx, ry;
unsigned buttons, dpad;
int l2, r2;
};
static bool parseFrame(char *line, Frame &f) {
char *star = strrchr(line, '*'); char *star = strrchr(line, '*');
if (!star) return false; if (!star) return false;
*star = '\0'; *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 (sscanf(star + 1, "%2x", &expected) != 1) return false;
if (xorChecksum(line, strlen(line)) != (uint8_t)expected) 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() { void setup() {
Serial.begin(115200); Serial.begin(115200);
Serial2.begin(LINK_BAUD, SERIAL_8N1, LINK_RX_PIN, LINK_TX_PIN); Serial2.begin(LINK_BAUD, SERIAL_8N1, LINK_RX_PIN, LINK_TX_PIN);
@@ -164,17 +269,15 @@ void setup() {
} }
void loop() { void loop() {
// 1. Keep the hubs connected, tracks first. // 1. Keep the hubs connected, hub0 first.
serviceHub(hubTracks, HUB_TRACKS_ADDR, tracksInitialised, tracksRetryAt, "tracks"); serviceHub(gHubs[0]);
if (hubTracks.isConnected()) { if (gHubs[0].hub.isConnected()) serviceHub(gHubs[1]);
serviceHub(hubUpperBody, HUB_UPPERBODY_ADDR, upperInitialised, upperRetryAt, "upper");
}
digitalWrite(STATUS_LED_PIN, bool ready = gHubs[0].hub.isConnected() && gHubs[1].hub.isConnected();
(hubTracks.isConnected() && hubUpperBody.isConnected()) ? HIGH : LOW); digitalWrite(STATUS_LED_PIN, ready ? HIGH : LOW);
// 2. Pull whole lines off the link. // 2. Pull whole lines off the link.
static char buf[96]; static char buf[128];
static size_t idx = 0; static size_t idx = 0;
while (Serial2.available()) { while (Serial2.available()) {
@@ -182,19 +285,11 @@ void loop() {
if (c == '\r') continue; if (c == '\r') continue;
if (c == '\n') { if (c == '\n') {
buf[idx] = '\0'; buf[idx] = '\0';
int lx, ly, rx, ry; Frame f;
unsigned buttons; if (idx > 0 && parseFrame(buf, f)) {
if (idx > 0 && parseFrame(buf, lx, ly, rx, ry, buttons)) {
lastFrameAt = millis(); lastFrameAt = millis();
failsafeEngaged = false; failsafeEngaged = false;
applyFrame(f);
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; idx = 0;
} else if (idx < sizeof(buf) - 1) { } else if (idx < sizeof(buf) - 1) {