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.

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
+// Hub BLE addresses, lower case.
-// repo, or with any BLE app on your phone - do not guess.
+//
+//   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:e6:8c",   // hub 0 - tracks + body lift
-    "90:84:2b:61:f2:d7",   // hub 1 - upper body
+    "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
+// Per-axis power caps. Everything except the tracks runs into a mechanical end
-static const uint32_t MOTOR_MIN_GAP_MS   = 60;   // per-port command throttle
+// stop, and there is no position feedback here - holding a direction at a stop
-static const uint32_t RECONNECT_GAP_MS   = 3000;
+// 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 leftTrack;    // hub 0 port B
-    volatile int rightTrack;
+    volatile int rightTrack;   // hub 0 port A
-    volatile int head;
+    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 <port> 11 51 00 <power>
 //   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);
+            gatt_event_characteristic_query_result_get_characteristic(
-            h.valueHandle = ch.value_handle;
+                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, &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
+                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_A, gDesired.rightTrack, gThrottle[0]);
-    driveMotor(gHubs[0], PORT_B, gDesired.rightTrack, gThrottle[1]);
+    driveMotor(gHubs[0], PORT_B, gDesired.leftTrack,  gThrottle[1]);
-    driveMotor(gHubs[1], PORT_A, gDesired.head,       gThrottle[2]);
+    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;
+        clearDesired();
-        gDesired.rightTrack = 0;
+    } else if (gp->b()) {                  // Circle - all stop
-        gDesired.head = 0;
+        clearDesired();
     } else {
-        gDesired.leftTrack  = stickToPower(-gp->axisY(),  TRACK_MAX);
+#if DEBUG_BUTTONS
-        gDesired.rightTrack = stickToPower(-gp->axisRY(), TRACK_MAX);
+        static uint16_t lastBtn = 0;
-        gDesired.head       = stickToPower(gp->axisX(),   HEAD_MAX);
+        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);