diff --git a/ps4_lego_onebrain/ps4_lego_onebrain.ino b/ps4_lego_onebrain/ps4_lego_onebrain.ino new file mode 100644 index 0000000..5293c76 --- /dev/null +++ b/ps4_lego_onebrain/ps4_lego_onebrain.ino @@ -0,0 +1,399 @@ +/* + * 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. + * + * Board package: esp32_bluepad32 (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. + * + * 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. Find them with the scanner in the two-board +// repo, or with any BLE app on your phone - do not guess. +static const char *HUB_ADDR_STR[2] = { + "90:84:2b:61:e6:8c", // hub 0 - tracks + "90:84:2b:61:f2:d7", // hub 1 - upper body +}; + +// 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 + +static const uint8_t PORT_A = 0x00; +static const uint8_t PORT_B = 0x01; +static const uint8_t PORT_LED = 0x32; + +static const int DEADZONE = 40; // raw stick counts, Bluepad32 range is +/-512 +static const int TRACK_MAX = 100; // LWP3 power range is -100..100 +static const int HEAD_MAX = 60; + +static const uint32_t TICK_MS = 25; // BTstack timer period +static const uint32_t MOTOR_MIN_GAP_MS = 60; // per-port command throttle +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; + volatile int rightTrack; + volatile int head; +}; +static DesiredState gDesired = {0, 0, 0}; + +static volatile bool gHubsReady = false; + +// ============================================================ hub plumbing + +enum HubState { + HUB_IDLE, + HUB_CONNECTING, + HUB_W4_SERVICE, + HUB_W4_CHARACTERISTIC, + HUB_READY, +}; + +struct Hub { + bd_addr_t addr; + HubState state; + hci_con_handle_t conHandle; + gatt_client_service_t service; + uint16_t valueHandle; + uint32_t retryAt; +}; + +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[3] = {{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 + +// 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); + if (!stopping) { + if (power == t.lastPower) return; + if ((now - t.lastSentAt) < MOTOR_MIN_GAP_MS) return; + } + + lwp3SendPower(h, port, (int8_t)power); + t.lastPower = power; + t.lastSentAt = 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]; + gatt_client_characteristic_t ch; + + 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, &ch); + h.valueHandle = ch.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; + } + 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; + + 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 < 3; 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.leftTrack, gThrottle[0]); + driveMotor(gHubs[0], PORT_B, gDesired.rightTrack, gThrottle[1]); + driveMotor(gHubs[1], PORT_A, gDesired.head, gThrottle[2]); +} + +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; + } + + 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"); +} + +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) { + gDesired.leftTrack = 0; + gDesired.rightTrack = 0; + gDesired.head = 0; + } else { + gDesired.leftTrack = stickToPower(-gp->axisY(), TRACK_MAX); + gDesired.rightTrack = stickToPower(-gp->axisRY(), TRACK_MAX); + gDesired.head = stickToPower(gp->axisX(), HEAD_MAX); + } + + vTaskDelay(1); +}