jessikitty/ps4-lego-onebrain
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
8532393a19b3fa41da560567045078ea667b5350
ps4-lego-onebrain/ps4_lego_onebrain/ps4_lego_onebrain.ino
T
jessikitty 3e553332eb Add port auto-detect via notifications, and the Johnny 5 control mapping 
- Subscribe to the LWP3 characteristic and decode Hub Attached I/O (0x04)
  so the hub reports its own port inventory at connect, plus Generic Error
  (0x05) so rejected commands stop vanishing silently.
- Map all 7 motors: arcade drive, head tilt/turn, body lift on the analog
  triggers, arms on d-pad and face buttons.
- Add a per-hub rate limit. Four motors on hub 1 exceeded what the per-port
  throttle alone would hold back.
- Fix track sides: port A is right, port B is left.
2026-09-01 23:22:48 +10:00

589 lines
22 KiB
Arduino
Raw Blame History

/*
* ps4_lego_onebrain.ino
*
* One ESP32. PS4 controller over Bluetooth Classic (Bluepad32/BTstack) AND two
* LEGO Powered Up hubs over BLE, driven by a hand-rolled LWP3 GATT client that
* runs on the same BTstack instance.
*
* Model: Johnny 5 (Short Circuit) MOC, 7 motors across 2 hubs.
*
* Board package: esp32-bluepad32 4.1.0 (NOT the plain esp32 package)
* Libraries: none. No Legoino, no NimBLE - they would be a second host
* stack and that is exactly what we are avoiding.
*
* BEFORE THIS WILL COMPILE:
* The board package ships the BTstack headers but does not put them on the
* include path. You must add a platform.local.txt to the package folder
* first - see docs/BUILD.md. Without it you get:
* fatal error: btstack.h: No such file or directory
*
* THREADING - read this before changing anything:
* BTstack is not thread-safe. loop() runs in the Arduino task, BTstack runs
* in its own task. The only BTstack call allowed from loop() is
* btstack_run_loop_execute_on_main_thread(). Everything LEGO-related here
* therefore runs inside a repeating BTstack timer (legoTickHandler), which
* executes on the BTstack thread. loop() only ever writes plain ints into
* gDesired; the timer reads them.
*
* BOOT ORDER:
* Gamepad discovery starts disabled. We connect both hubs first, then turn
* discovery on. This stops Bluepad32's inquiry/scan from colliding with our
* LE create-connection.
*
* Created by: Jess Rogerson (yelling commands at Claude.AI)
*/
#include <Bluepad32.h>
#include <btstack.h>
// =========================================================== configuration
// Hub BLE addresses, lower case.
//
// 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 *HUB_ADDR_STR[2] = {
"90:84:2b:61:e6:8c", // hub 0 - tracks + body lift
"90:84:2b:61:f2:d7", // hub 1 - head + arms
};
// LEGO hubs use a public address (90:84:2B is LEGO's OUI). If the connect
// never completes, try BD_ADDR_TYPE_LE_RANDOM here.
#define HUB_ADDR_TYPE BD_ADDR_TYPE_LE_PUBLIC
// Set to 1 to print the button mask whenever it changes, for remapping.
#define DEBUG_BUTTONS 0
static const uint8_t PORT_A = 0x00;
static const uint8_t PORT_B = 0x01;
static const uint8_t PORT_C = 0x02;
static const uint8_t PORT_D = 0x03;
static const uint8_t PORT_LED = 0x32;
static const int DEADZONE = 40; // raw stick counts, Bluepad32 range is +/-512
// Per-axis power caps. Everything except the tracks runs into a mechanical end
// stop, and there is no position feedback here - holding a direction at a stop
// stalls the motor. Keep these conservative; lower them if an axis feels forceful.
static const int TRACK_MAX = 100; // LWP3 power range is -100..100
static const int HEAD_MAX = 50;
static const int LIFT_MAX = 60;
static const int ARM_MAX = 45;
// Drive 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;
static const uint32_t TICK_MS = 25; // BTstack timer period
static const uint32_t MOTOR_MIN_GAP_MS = 100; // per-port command throttle
static const uint32_t HUB_MIN_GAP_MS = 25; // per-hub floor, ~40 cmd/sec
static const uint32_t RECONNECT_GAP_MS = 3000;
static const int STATUS_LED_PIN = 2;
// LWP3 UUIDs, big-endian byte order as BTstack expects.
static const uint8_t LWP3_SERVICE_UUID[16] = {
0x00, 0x00, 0x16, 0x23, 0x12, 0x12, 0xef, 0xde,
0x16, 0x23, 0x78, 0x5f, 0xea, 0xbc, 0xd1, 0x23};
static const uint8_t LWP3_CHAR_UUID[16] = {
0x00, 0x00, 0x16, 0x24, 0x12, 0x12, 0xef, 0xde,
0x16, 0x23, 0x78, 0x5f, 0xea, 0xbc, 0xd1, 0x23};
// ============================================== shared between the 2 tasks
// Written by loop() (Arduino task), read by the BTstack timer. 32-bit aligned
// int stores on the ESP32 are single instructions, so a torn read is not
// possible; the worst case is one tick of stale data, which we do not care
// about at 40 Hz.
struct DesiredState {
volatile int leftTrack; // hub 0 port B
volatile int rightTrack; // hub 0 port A
volatile int bodyLift; // hub 0 port D
volatile int headTilt; // hub 1 port A
volatile int headTurn; // hub 1 port B
volatile int leftArm; // hub 1 port C
volatile int rightArm; // hub 1 port D
};
static DesiredState gDesired = {0, 0, 0, 0, 0, 0, 0};
static volatile bool gHubsReady = false;
static void clearDesired() {
gDesired.leftTrack = 0;
gDesired.rightTrack = 0;
gDesired.bodyLift = 0;
gDesired.headTilt = 0;
gDesired.headTurn = 0;
gDesired.leftArm = 0;
gDesired.rightArm = 0;
}
// ============================================================ hub plumbing
enum HubState {
HUB_IDLE,
HUB_CONNECTING,
HUB_W4_SERVICE,
HUB_W4_CHARACTERISTIC,
HUB_W4_CCC,
HUB_READY,
};
struct Hub {
bd_addr_t addr;
HubState state;
hci_con_handle_t conHandle;
gatt_client_service_t service;
gatt_client_characteristic_t characteristic;
gatt_client_notification_t notification;
uint16_t valueHandle;
uint32_t retryAt;
uint32_t lastCmdAt; // per-hub rate limit, shared across its ports
};
static Hub gHubs[2];
static int gActive = -1; // hub currently mid-discovery, -1 if none
struct PortThrottle {
int lastPower;
uint32_t lastSentAt;
};
static PortThrottle gThrottle[7] = {{999, 0}, {999, 0}, {999, 0}, {999, 0},
{999, 0}, {999, 0}, {999, 0}};
static btstack_packet_callback_registration_t gHciRegistration;
static btstack_context_callback_registration_t gBootstrapRegistration;
static btstack_timer_source_t gLegoTimer;
// ============================================================== gamepad io
ControllerPtr myControllers[BP32_MAX_CONTROLLERS];
static bool gDiscoveryEnabled = false;
void onConnectedController(ControllerPtr ctl) {
for (int i = 0; i < BP32_MAX_CONTROLLERS; i++) {
if (myControllers[i] == nullptr) {
myControllers[i] = ctl;
return;
}
}
}
void onDisconnectedController(ControllerPtr ctl) {
for (int i = 0; i < BP32_MAX_CONTROLLERS; i++) {
if (myControllers[i] == ctl) {
myControllers[i] = nullptr;
return;
}
}
}
// Deadzone, then rescale so the remaining travel still reaches full power.
static int stickToPower(int raw, int maxPower) {
if (raw > -DEADZONE && raw < DEADZONE) return 0;
int sign = (raw < 0) ? -1 : 1;
long mag = labs((long)raw) - DEADZONE;
long scaled = (mag * maxPower) / (512L - DEADZONE);
if (scaled > maxPower) scaled = maxPower;
return sign * (int)scaled;
}
// ======================================= LWP3 - all of this on BTstack task
// I/O device type IDs, from pybricks/technical-info assigned-numbers.md.
static const char *deviceTypeName(uint16_t id) {
switch (id) {
case 0x0001: return "Powered Up medium motor";
case 0x0002: return "train motor";
case 0x0008: return "Powered Up light";
case 0x0014: return "battery voltage";
case 0x0015: return "battery current";
case 0x0016: return "piezo tone";
case 0x0017: return "hub RGB LED";
case 0x0025: return "BOOST colour/distance sensor";
case 0x0026: return "BOOST interactive motor";
case 0x0027: return "BOOST built-in motor";
case 0x002E: return "Technic Control+ LARGE motor";
case 0x002F: return "Technic Control+ XL motor";
case 0x0030: return "SPIKE Prime medium motor";
case 0x0031: return "SPIKE Prime large motor";
case 0x0036: return "hub IMU gesture";
case 0x0039: return "hub IMU accelerometer";
case 0x003A: return "hub IMU gyro";
case 0x003B: return "hub IMU position";
case 0x003C: return "hub IMU temperature";
case 0x003D: return "Technic colour sensor";
case 0x003E: return "Technic distance sensor";
case 0x003F: return "Technic force sensor";
case 0x0041: return "Technic small angular motor";
case 0x004B: return "Technic medium angular motor (grey)";
case 0x004C: return "Technic large angular motor (grey)";
default: return "unknown";
}
}
// Decodes the messages the hub pushes at us. Runs on the BTstack thread, so
// keep it cheap - the printing here is fine because it only fires at connect
// time, but do not add prints to anything that runs per motor command.
static void onHubMessage(int idx, const uint8_t *msg, uint16_t len) {
if (len < 3) return;
if (msg[2] == 0x04 && len >= 5) { // Hub Attached I/O
uint8_t port = msg[3], event = msg[4];
char portName[8];
if (port <= 0x03) snprintf(portName, sizeof(portName), "%c", 'A' + port);
else snprintf(portName, sizeof(portName), "0x%02X", port);
if (event == 0x00) {
Serial.printf("hub%d port %s : detached\n", idx, portName);
} else if (len >= 7) {
uint16_t t = msg[5] | (msg[6] << 8);
Serial.printf("hub%d port %s : %s (0x%04X)%s\n", idx, portName,
deviceTypeName(t), t, event == 0x02 ? " [virtual]" : "");
}
} else if (msg[2] == 0x05 && len >= 5) { // Generic Error
Serial.printf("hub%d ERROR: command 0x%02X rejected, code 0x%02X\n",
idx, msg[3], msg[4]);
}
}
static void notificationHandler(uint8_t packet_type, uint16_t channel,
uint8_t *packet, uint16_t size) {
if (hci_event_packet_get_type(packet) != GATT_EVENT_NOTIFICATION) return;
hci_con_handle_t handle = gatt_event_notification_get_handle(packet);
for (int i = 0; i < 2; i++) {
if (gHubs[i].conHandle != handle) continue;
onHubMessage(i, gatt_event_notification_get_value(packet),
gatt_event_notification_get_value_length(packet));
return;
}
}
// Port Output Command / WriteDirectModeData:
// 08 00 81 <port> 11 51 00 <power>
// len, hubID, msgType, port, startup+completion, subcmd, mode, payload
static void lwp3SendPower(Hub &h, uint8_t port, int8_t power) {
if (h.state != HUB_READY) return;
uint8_t msg[8] = {0x08, 0x00, 0x81, port, 0x11, 0x51, 0x00, (uint8_t)power};
gatt_client_write_value_of_characteristic_without_response(
h.conHandle, h.valueHandle, sizeof(msg), msg);
}
static void lwp3SetLed(Hub &h, uint8_t colour) {
if (h.state != HUB_READY) return;
uint8_t msg[8] = {0x08, 0x00, 0x81, PORT_LED, 0x11, 0x51, 0x00, colour};
gatt_client_write_value_of_characteristic_without_response(
h.conHandle, h.valueHandle, sizeof(msg), msg);
}
static void driveMotor(Hub &h, uint8_t port, int power, PortThrottle &t) {
if (h.state != HUB_READY) return;
uint32_t now = btstack_run_loop_get_time_ms();
bool stopping = (power == 0 && t.lastPower != 0);
// Stops always go out immediately. Everything else is rate limited twice:
// per port, and per hub - with 4 motors on hub 1 the per-port limit alone
// still lets through more than the hub will swallow.
if (!stopping) {
if (power == t.lastPower) return;
if ((now - t.lastSentAt) < MOTOR_MIN_GAP_MS) return;
if ((now - h.lastCmdAt) < HUB_MIN_GAP_MS) return;
}
lwp3SendPower(h, port, (int8_t)power);
t.lastPower = power;
t.lastSentAt = now;
h.lastCmdAt = now;
}
static void gattPacketHandler(uint8_t packet_type, uint16_t channel,
uint8_t *packet, uint16_t size) {
if (gActive < 0) return;
Hub &h = gHubs[gActive];
switch (hci_event_packet_get_type(packet)) {
case GATT_EVENT_SERVICE_QUERY_RESULT:
gatt_event_service_query_result_get_service(packet, &h.service);
break;
case GATT_EVENT_CHARACTERISTIC_QUERY_RESULT:
gatt_event_characteristic_query_result_get_characteristic(
packet, &h.characteristic);
h.valueHandle = h.characteristic.value_handle;
break;
case GATT_EVENT_QUERY_COMPLETE:
if (h.state == HUB_W4_SERVICE) {
if (h.service.start_group_handle == 0) {
// Not a LEGO hub, or it never answered.
gap_disconnect(h.conHandle);
break;
}
h.state = HUB_W4_CHARACTERISTIC;
gatt_client_discover_characteristics_for_service_by_uuid128(
&gattPacketHandler, h.conHandle, &h.service, LWP3_CHAR_UUID);
} else if (h.state == HUB_W4_CHARACTERISTIC) {
if (h.valueHandle == 0) {
gap_disconnect(h.conHandle);
break;
}
// Register the listener BEFORE subscribing. The hub dumps its
// whole port inventory the instant notifications go live, and
// we would miss it otherwise.
gatt_client_listen_for_characteristic_value_updates(
&h.notification, &notificationHandler, h.conHandle,
&h.characteristic);
h.state = HUB_W4_CCC;
uint8_t st = gatt_client_write_client_characteristic_configuration(
&gattPacketHandler, h.conHandle, &h.characteristic,
GATT_CLIENT_CHARACTERISTICS_CONFIGURATION_NOTIFICATION);
if (st != ERROR_CODE_SUCCESS) {
Serial.printf("hub%d CCC write failed: 0x%02X\n", gActive, st);
h.state = HUB_READY; // motors still work, just no reports
gActive = -1;
lwp3SetLed(h, 0x06);
}
} else if (h.state == HUB_W4_CCC) {
Serial.printf("hub%d ready, listening\n", gActive);
h.state = HUB_READY;
gActive = -1;
lwp3SetLed(h, 0x06); // green
}
break;
default:
break;
}
}
static void onLeConnected(hci_con_handle_t handle) {
if (gActive < 0) return;
Hub &h = gHubs[gActive];
if (h.state != HUB_CONNECTING) return; // not ours - probably a BLE gamepad
h.conHandle = handle;
h.state = HUB_W4_SERVICE;
memset(&h.service, 0, sizeof(h.service));
h.valueHandle = 0;
gatt_client_discover_primary_services_by_uuid128(
&gattPacketHandler, handle, LWP3_SERVICE_UUID);
}
static void onDisconnected(hci_con_handle_t handle) {
for (int i = 0; i < 2; i++) {
if (gHubs[i].state == HUB_IDLE) continue;
if (gHubs[i].conHandle != handle) continue;
gatt_client_stop_listening_for_characteristic_value_updates(
&gHubs[i].notification);
gHubs[i].state = HUB_IDLE;
gHubs[i].conHandle = HCI_CON_HANDLE_INVALID;
gHubs[i].valueHandle = 0;
gHubs[i].retryAt = btstack_run_loop_get_time_ms() + RECONNECT_GAP_MS;
gHubsReady = false;
if (gActive == i) gActive = -1;
for (int p = 0; p < 7; p++) gThrottle[p].lastPower = 999;
}
}
static void hciPacketHandler(uint8_t packet_type, uint16_t channel,
uint8_t *packet, uint16_t size) {
if (packet_type != HCI_EVENT_PACKET) return;
switch (hci_event_packet_get_type(packet)) {
#ifdef HCI_EVENT_META_GAP
case HCI_EVENT_META_GAP:
if (hci_event_gap_meta_get_subevent_code(packet) !=
GAP_SUBEVENT_LE_CONNECTION_COMPLETE)
break;
onLeConnected(
gap_subevent_le_connection_complete_get_connection_handle(packet));
break;
#endif
case HCI_EVENT_LE_META:
if (hci_event_le_meta_get_subevent_code(packet) !=
HCI_SUBEVENT_LE_CONNECTION_COMPLETE)
break;
onLeConnected(
hci_subevent_le_connection_complete_get_connection_handle(packet));
break;
case HCI_EVENT_DISCONNECTION_COMPLETE:
onDisconnected(
hci_event_disconnection_complete_get_connection_handle(packet));
break;
default:
break;
}
}
// One hub at a time. The controller only allows a single outstanding LE
// create-connection, and doing them in sequence keeps this simple.
static void serviceHubConnections() {
if (gActive >= 0) return;
uint32_t now = btstack_run_loop_get_time_ms();
for (int i = 0; i < 2; i++) {
if (gHubs[i].state != HUB_IDLE) continue;
if (now < gHubs[i].retryAt) continue;
gActive = i;
gHubs[i].state = HUB_CONNECTING;
gHubs[i].retryAt = now + RECONNECT_GAP_MS;
gap_connect(gHubs[i].addr, HUB_ADDR_TYPE);
return;
}
}
static void legoTick() {
serviceHubConnections();
// A connect attempt that never completes leaves us stuck on gActive.
if (gActive >= 0 && gHubs[gActive].state == HUB_CONNECTING &&
btstack_run_loop_get_time_ms() > gHubs[gActive].retryAt) {
gap_connect_cancel();
gHubs[gActive].state = HUB_IDLE;
gHubs[gActive].retryAt = btstack_run_loop_get_time_ms() + RECONNECT_GAP_MS;
gActive = -1;
return;
}
gHubsReady = (gHubs[0].state == HUB_READY && gHubs[1].state == HUB_READY);
driveMotor(gHubs[0], PORT_A, gDesired.rightTrack, gThrottle[0]);
driveMotor(gHubs[0], PORT_B, gDesired.leftTrack, gThrottle[1]);
driveMotor(gHubs[0], PORT_D, gDesired.bodyLift, gThrottle[2]);
driveMotor(gHubs[1], PORT_A, gDesired.headTilt, gThrottle[3]);
driveMotor(gHubs[1], PORT_B, gDesired.headTurn, gThrottle[4]);
driveMotor(gHubs[1], PORT_C, gDesired.leftArm, gThrottle[5]);
driveMotor(gHubs[1], PORT_D, gDesired.rightArm, gThrottle[6]);
}
static void legoTickHandler(btstack_timer_source_t *ts) {
legoTick();
btstack_run_loop_set_timer(ts, TICK_MS);
btstack_run_loop_add_timer(ts);
}
// Runs once, on the BTstack thread, scheduled from setup().
static void legoBootstrap(void *context) {
(void)context;
gatt_client_init();
gHciRegistration.callback = &hciPacketHandler;
hci_add_event_handler(&gHciRegistration);
gLegoTimer.process = &legoTickHandler;
btstack_run_loop_set_timer(&gLegoTimer, TICK_MS);
btstack_run_loop_add_timer(&gLegoTimer);
}
// =================================================================== setup
void setup() {
Serial.begin(115200);
pinMode(STATUS_LED_PIN, OUTPUT);
digitalWrite(STATUS_LED_PIN, LOW);
for (int i = 0; i < 2; i++) {
sscanf_bd_addr(HUB_ADDR_STR[i], gHubs[i].addr);
gHubs[i].state = HUB_IDLE;
gHubs[i].conHandle = HCI_CON_HANDLE_INVALID;
gHubs[i].valueHandle = 0;
gHubs[i].retryAt = 0;
gHubs[i].lastCmdAt = 0;
}
BP32.setup(&onConnectedController, &onDisconnectedController);
BP32.enableVirtualDevice(false);
// Hubs first. Gamepad discovery gets switched on once they are up.
BP32.enableNewBluetoothConnections(false);
gBootstrapRegistration.callback = &legoBootstrap;
gBootstrapRegistration.context = NULL;
btstack_run_loop_execute_on_main_thread(&gBootstrapRegistration);
Serial.println("Connecting hubs, then opening for the gamepad");
}
// ==================================================================== loop
void loop() {
BP32.update();
if (gHubsReady && !gDiscoveryEnabled) {
BP32.enableNewBluetoothConnections(true);
gDiscoveryEnabled = true;
Serial.println("Hubs up - put the PS4 pad in pairing mode (SHARE + PS)");
}
ControllerPtr gp = nullptr;
for (int i = 0; i < BP32_MAX_CONTROLLERS; i++) {
if (myControllers[i] && myControllers[i]->isConnected() &&
myControllers[i]->isGamepad()) {
gp = myControllers[i];
break;
}
}
digitalWrite(STATUS_LED_PIN, (gp && gHubsReady) ? HIGH : LOW);
if (gp == nullptr) {
clearDesired();
} else if (gp->b()) { // Circle - all stop
clearDesired();
} else {
#if DEBUG_BUTTONS
static uint16_t lastBtn = 0;
static uint8_t lastDpad = 0;
if (gp->buttons() != lastBtn || gp->dpad() != lastDpad) {
lastBtn = gp->buttons();
lastDpad = gp->dpad();
Serial.printf("buttons=0x%04x dpad=0x%02x L2=%d R2=%d\n",
lastBtn, lastDpad, gp->brake(), gp->throttle());
}
#endif
// Arcade drive on the left stick. One stick for driving frees the
// right one entirely for the head, which is what Johnny emotes with.
int fwd = stickToPower(-gp->axisY(), TRACK_MAX);
int turn = stickToPower(gp->axisX(), TRACK_MAX);
int scale = SCALE_NORMAL;
if (gp->l1()) scale = SCALE_PRECISION;
if (gp->r1()) scale = SCALE_FULL;
fwd = fwd * scale / 100;
turn = turn * scale / 100;
gDesired.leftTrack = constrain(fwd + turn, -TRACK_MAX, TRACK_MAX);
gDesired.rightTrack = constrain(fwd - turn, -TRACK_MAX, TRACK_MAX);
gDesired.headTurn = stickToPower(gp->axisRX(), HEAD_MAX);
gDesired.headTilt = stickToPower(-gp->axisRY(), HEAD_MAX);
// R2 raises, L2 lowers. Both are analog, 0..1023. Proportional control
// suits the heavy slow axis better than the sticks would.
gDesired.bodyLift = constrain(
(gp->throttle() - gp->brake()) * LIFT_MAX / 1023, -LIFT_MAX, LIFT_MAX);
// Arms are pose-and-hold rather than modulate, so digital is fine.
uint8_t d = gp->dpad();
gDesired.leftArm = (d & DPAD_UP) ? ARM_MAX : (d & DPAD_DOWN) ? -ARM_MAX : 0;
gDesired.rightArm = gp->y() ? ARM_MAX : gp->a() ? -ARM_MAX : 0;
}
vTaskDelay(1);
}
Reference in New Issue View Git Blame Copy Permalink