diff --git a/ps4_lego_onebrain/ps4_lego_onebrain.ino b/ps4_lego_onebrain/ps4_lego_onebrain.ino index 586f89d..2502073 100644 --- a/ps4_lego_onebrain/ps4_lego_onebrain.ino +++ b/ps4_lego_onebrain/ps4_lego_onebrain.ino @@ -5,6 +5,8 @@ * 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. @@ -36,28 +38,50 @@ // =========================================================== 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. +// 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 - "90:84:2b:61:f2:d7", // hub 1 - upper body + "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 -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; +// 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; @@ -76,14 +100,28 @@ static const uint8_t LWP3_CHAR_UUID[16] = { // 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; + 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}; +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 { @@ -91,6 +129,7 @@ enum HubState { HUB_CONNECTING, HUB_W4_SERVICE, HUB_W4_CHARACTERISTIC, + HUB_W4_CCC, HUB_READY, }; @@ -99,8 +138,11 @@ struct Hub { 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]; @@ -110,7 +152,8 @@ struct PortThrottle { int lastPower; uint32_t lastSentAt; }; -static PortThrottle gThrottle[3] = {{999, 0}, {999, 0}, {999, 0}}; +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; @@ -151,6 +194,76 @@ static int stickToPower(int raw, int maxPower) { // ======================================= 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 @@ -173,21 +286,26 @@ static void driveMotor(Hub &h, uint8_t port, int power, PortThrottle &t) { 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]; - gatt_client_characteristic_t ch; switch (hci_event_packet_get_type(packet)) { case GATT_EVENT_SERVICE_QUERY_RESULT: @@ -195,8 +313,9 @@ static void gattPacketHandler(uint8_t packet_type, uint16_t channel, break; case GATT_EVENT_CHARACTERISTIC_QUERY_RESULT: - gatt_event_characteristic_query_result_get_characteristic(packet, &ch); - h.valueHandle = ch.value_handle; + gatt_event_characteristic_query_result_get_characteristic( + packet, &h.characteristic); + h.valueHandle = h.characteristic.value_handle; break; case GATT_EVENT_QUERY_COMPLETE: @@ -214,9 +333,29 @@ static void gattPacketHandler(uint8_t packet_type, uint16_t channel, 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 + lwp3SetLed(h, 0x06); // green } break; @@ -243,13 +382,16 @@ static void onDisconnected(hci_con_handle_t handle) { 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 < 3; p++) gThrottle[p].lastPower = 999; + for (int p = 0; p < 7; p++) gThrottle[p].lastPower = 999; } } @@ -318,9 +460,13 @@ static void legoTick() { 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]); + 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) { @@ -356,6 +502,7 @@ void setup() { gHubs[i].conHandle = HCI_CON_HANDLE_INVALID; gHubs[i].valueHandle = 0; gHubs[i].retryAt = 0; + gHubs[i].lastCmdAt = 0; } BP32.setup(&onConnectedController, &onDisconnectedController); @@ -371,6 +518,8 @@ void setup() { Serial.println("Connecting hubs, then opening for the gamepad"); } +// ==================================================================== loop + void loop() { BP32.update(); @@ -392,13 +541,47 @@ void loop() { digitalWrite(STATUS_LED_PIN, (gp && gHubsReady) ? HIGH : LOW); if (gp == nullptr) { - gDesired.leftTrack = 0; - gDesired.rightTrack = 0; - gDesired.head = 0; + clearDesired(); + } else if (gp->b()) { // Circle - all stop + clearDesired(); } else { - gDesired.leftTrack = stickToPower(-gp->axisY(), TRACK_MAX); - gDesired.rightTrack = stickToPower(-gp->axisRY(), TRACK_MAX); - gDesired.head = stickToPower(gp->axisX(), HEAD_MAX); +#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);