/* * 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. * * CONTROL SCHEME: tank drive. Every input drives exactly one motor - no * mixing. See docs/CONTROLS.md. * * 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 * * FILE ORDER MATTERS: * The Arduino IDE generates function prototypes and injects them immediately * before the FIRST function definition in the file. Any type used in a * function signature must therefore be declared above that point. All the * structs and enums live at the top for this reason. Move a function above * them and you get a wall of "'Hub' was not declared in this scope". * * 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 #include // =========================================================== 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 = 45; // digital now, so this is the only speed 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; 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}; // ===================================================================== types // // Everything the Arduino prototype generator might need. Keep this block above // the first function definition in the file - see the note in the header. 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 }; 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 }; struct PortThrottle { int lastPower; uint32_t lastSentAt; }; // ================================================================== globals // gDesired is written by loop() (Arduino task) and 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. static DesiredState gDesired = {0, 0, 0, 0, 0, 0, 0}; static volatile bool gHubsReady = false; static Hub gHubs[2]; static int gActive = -1; // hub currently mid-discovery, -1 if none 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; ControllerPtr myControllers[BP32_MAX_CONTROLLERS]; static bool gDiscoveryEnabled = false; // ============================================================== gamepad io // // First function definition in the file. Everything above this line is types // and data, which is what makes the generated prototypes compile. static void clearDesired() { gDesired.leftTrack = 0; gDesired.rightTrack = 0; gDesired.bodyLift = 0; gDesired.headTilt = 0; gDesired.headTurn = 0; gDesired.leftArm = 0; gDesired.rightArm = 0; } 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 11 51 00 // 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, ¬ificationHandler, 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); // Uncomment, flash once, re-pair, then comment out again if you ever end // up with stale pairings. Bluepad32 keeps these in NVS and reflashing the // sketch does not clear them. // BP32.forgetBluetoothKeys(); // 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->l1() && gp->r1()) { // both shoulders together - 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 // Tank drive. One stick per track, vertical axis only. Nothing is // mixed, so a stick can never command two motors. int scale = SCALE_NORMAL; if (gp->l1()) scale = SCALE_PRECISION; if (gp->r1()) scale = SCALE_FULL; gDesired.leftTrack = stickToPower(-gp->axisY(), TRACK_MAX) * scale / 100; gDesired.rightTrack = stickToPower(-gp->axisRY(), TRACK_MAX) * scale / 100; // Head on the d-pad. Up/down tilts, left/right turns. Pressing a // diagonal will move both, but only because you asked it to. uint8_t d = gp->dpad(); gDesired.headTilt = (d & DPAD_UP) ? HEAD_MAX : (d & DPAD_DOWN) ? -HEAD_MAX : 0; gDesired.headTurn = (d & DPAD_RIGHT) ? HEAD_MAX : (d & DPAD_LEFT) ? -HEAD_MAX : 0; // R2 raises, L2 lowers. Both are analog, 0..1023, so the lift keeps // proportional control - it is the heavy axis that benefits most. gDesired.bodyLift = constrain( (gp->throttle() - gp->brake()) * LIFT_MAX / 1023, -LIFT_MAX, LIFT_MAX); // Arms on the face buttons. Square/Circle left, Triangle/Cross right. gDesired.leftArm = gp->x() ? ARM_MAX : gp->b() ? -ARM_MAX : 0; gDesired.rightArm = gp->y() ? ARM_MAX : gp->a() ? -ARM_MAX : 0; } vTaskDelay(1); }