Restart hub scans that find nothing

A Legoino scan that never locates its hub ends quietly - isConnecting()
never goes true, so the old code left 'initialised' set and never touched
retryAt. That hub then sat on a dead scan until the board was power
cycled, which is why first connects needed several attempts.

Adds scanExpiresAt per hub and restarts the scan after 12s. Also
documents the startup order and the LED grounding in the header.

receiver/receiver.ino

@@ -1,32 +1,49 @@
 /*
  * receiver.ino  --  UART -> two LEGO Powered Up hubs (ESP32 "B")
  *
- * Model: Johnny 5 (Short Circuit) MOC, 7 motors across 2 Technic hubs.
+ * Model: Johnny 5 (Short Circuit) MOC - "Evolved" control scheme.
+ *        7 motors across 2 Technic hubs, plus 2 GPIO-driven LED circuits.
  *
  * Board package: esp32 (the normal Espressif one), core 2.0.17
  * Libraries:     Legoino  +  NimBLE-Arduino 1.4.x  (both via Library Manager)
  *
  * Core 3.x will not build Legoino - you get 'std::string does not name a type'
  * and a ReadUInt32LE declaration mismatch. Stay on 2.0.17 for this board.
- *
- * Listens for gamepad frames from the Bluepad32 board on Serial2 and drives
- * both hubs over BLE using Legoino. This board must NOT have the Bluepad32
- * board package selected - keeping BTstack and NimBLE on separate chips is
- * the entire reason there are two boards.
+ * Do NOT select the esp32_bluepad32 package here either: it starts BTstack
+ * before setup() runs and NimBLE aborts with ESP_ERR_INVALID_STATE.
  *
  * Wiring to the transmitter board:
  *   RX GPIO16  <-  TX GPIO17 on transmitter
  *   TX GPIO17  ->  RX GPIO16 on transmitter
  *   GND        ->  GND                       (mandatory - common ground)
  *
- * Control scheme is tank drive: every input drives exactly one motor.
+ * LED circuits - both on THIS board, returning to THIS board's GND:
+ *   GPIO25 -> resistor -> LED pair -> GND
+ *   GPIO26 -> resistor -> LED pair -> GND
+ *   Pins output 3.3V, not 3V. ~12mA per pin is comfortable, 40mA is the hard
+ *   limit. Anything drawing more than ~20mA per circuit needs a transistor.
+ *   Do not tap the LED return off the UART ground wire to the other board -
+ *   that reference needs to stay clean.
+ *
+ * STARTUP ORDER:
+ *   Wake BOTH hubs with their green buttons and check both are blinking,
+ *   THEN power this board. hub1 is only serviced once hub0 is connected, so a
+ *   sleeping hub0 blocks the whole sequence. Hubs stop advertising after a
+ *   couple of minutes idle.
+ *
+ * ARMS ARE POSITION CONTROLLED:
+ *   The triggers set an ANGLE, not a power level. Trigger released holds the
+ *   arm at 0, fully depressed holds it at ARM_SPAN_*. The encoders are zeroed
+ *   once, on the first hub1 connect after boot, so BOTH ARMS MUST BE DOWN at
+ *   that moment. They are deliberately NOT re-zeroed on a reconnect - the hub
+ *   keeps its encoder preset, and re-zeroing mid-session would redefine zero
+ *   at whatever position the arms happened to be in.
  *
  * FILE ORDER MATTERS:
  *   The Arduino IDE injects generated function prototypes immediately before
  *   the FIRST function definition in the file. Any type used in a function
  *   signature must be declared above that point - which is why HubLink,
- *   PortState and Frame all live in the types block. Move a function above
- *   them and you get "'Frame' has not been declared".
+ *   PortState, ArmState and Frame all live in the types block.
  *
  * Created by: Jess Rogerson (yelling commands at Claude.AI)
  */
@@ -38,55 +55,64 @@
 // Hub BLE addresses. Run tools/hub_scanner to find them - 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
+//     A  right track            A  left arm
+//     B  left track             B  head tilt
+//     C  (free)                 C  head turn
+//     D  body lift              D  right arm
 static const char *HUB0_ADDR = "90:84:2b:61:f2:d7";
 static const char *HUB1_ADDR = "90:84:2b:61:e6:8c";
 
-// Port numbers are just bytes in the LEGO protocol, same on every hub type.
 static const byte PORT_A = 0x00;
 static const byte PORT_B = 0x01;
 static const byte PORT_C = 0x02;
 static const byte PORT_D = 0x03;
 
-// Technic / Control+ motors are tacho motors -> leave this at 1.
-// Plain train motors and the simple Powered Up motors -> set it to 0.
+// LED circuits. Safe GPIOs - no boot strapping or flash duties, unlike 0, 2,
+// 12 and 15.
+static const int LED_1_PIN = 25;    // Circle toggles this pair
+static const int LED_2_PIN = 26;    // Triangle toggles this pair
+
 #define USE_TACHO_MOTORS 1
 
-// Set to 1 to log every motor command that goes out. Useful for proving which
-// port a command actually lands on. Noisy - turn it back off afterwards.
+// Set to 1 to log every command that goes out. Noisy - turn it back off.
 #define DEBUG_MOTORS 0
 
 static const int DEADZONE = 40;     // raw stick counts ignored around centre
 
-// Motor directions. Flip to -1 if an axis runs backwards - mirrored mountings
-// are normal on a symmetric model, and this is cheaper than editing signs
-// scattered through applyFrame().
+// Motor directions. Flip to -1 if an axis runs backwards.
 static const int DIR_LEFT_TRACK  =  1;
 static const int DIR_RIGHT_TRACK = -1;
 static const int DIR_BODY_LIFT   =  1;
 static const int DIR_HEAD_TILT   =  1;
 static const int DIR_HEAD_TURN   =  1;
-static const int DIR_LEFT_ARM    =  1;
-static const int DIR_RIGHT_ARM   =  1;
 
-// Per-axis power caps. Everything except the tracks runs into a mechanical end
-// stop and there is no position feedback, so holding a direction at a stop
-// stalls the motor. Lower these if an axis feels forceful.
+// Arm travel in MOTOR degrees, measured with tools/arm_calibrate.
+// Raw measurements were roughly +280 (left) and -275 (right). These are set
+// slightly short so backlash cannot stall the motor against the top stop.
+// The sign carries the direction - there is no DIR_ constant for the arms.
+static const int32_t ARM_SPAN_LEFT  =  250;
+static const int32_t ARM_SPAN_RIGHT = -250;
+
+static const int  ARM_SPEED     = 60;   // how fast it travels to the target
+static const byte ARM_MAX_POWER = 40;   // torque cap - keeps a jam survivable
+static const int  ARM_STEP_DEG  = 3;    // ignore target changes smaller than this
+
+// Arms get their own command interval. They are position controlled, so the
+// target moves continuously with the trigger and needs updating more often
+// than a velocity axis does - at the per-port 100ms the arm sprints to each
+// target then sits idle, which feels like stepping.
+static const unsigned long ARM_MIN_GAP_MS = 60;
+
+// Per-axis power caps for the velocity-controlled axes.
 static const int TRACK_MAX = 100;   // LEGO speed range is -100..100
 static const int HEAD_MAX  = 45;
 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;
+// Track speed, toggled by Cross.
+static const int SPEED_SLOW = 50;
+static const int SPEED_FAST = 100;
 
-// Bluepad32 button masks. Verify against your own pad with DEBUG_BUTTONS in
-// the transmitter sketch if any of these seem wrong.
+// Bluepad32 button masks. Verify with DEBUG_BUTTONS in the transmitter.
 static const unsigned BTN_A  = 0x0001;   // Cross
 static const unsigned BTN_B  = 0x0002;   // Circle
 static const unsigned BTN_X  = 0x0004;   // Square
@@ -99,11 +125,18 @@ static const unsigned DPAD_D = 0x02;
 static const unsigned DPAD_R = 0x04;
 static const unsigned DPAD_L = 0x08;
 
+// Triggers rest at 0 and are noisy near the bottom of their travel.
+static const int TRIGGER_DEADZONE = 60;
+
 static const unsigned long MOTOR_MIN_GAP_MS = 100;  // per port
 static const unsigned long HUB_MIN_GAP_MS   = 25;   // per hub, ~40 cmd/sec
 static const unsigned long LINK_TIMEOUT_MS  = 400;  // failsafe
 static const unsigned long RECONNECT_GAP_MS = 2000;
 
+// A Legoino scan that never finds its hub expires silently, leaving the hub
+// stuck waiting forever. This is how long we give it before starting over.
+static const unsigned long SCAN_TIMEOUT_MS = 12000;
+
 static const int  LINK_RX_PIN = 16;
 static const int  LINK_TX_PIN = 17;
 static const long LINK_BAUD   = 115200;
@@ -118,7 +151,8 @@ struct HubLink {
     const char *label;
     bool initialised;
     unsigned long retryAt;
-    unsigned long lastCmdAt;   // per-hub rate limit, shared across its ports
+    unsigned long lastCmdAt;       // per-hub rate limit, shared across ports
+    unsigned long scanExpiresAt;   // when to give up on the current scan
 };
 
 struct PortState {
@@ -126,6 +160,11 @@ struct PortState {
     unsigned long lastSentAt;
 };
 
+struct ArmState {
+    int32_t lastTarget;
+    unsigned long lastSentAt;
+};
+
 struct Frame {
     int lx, ly, rx, ry;
     unsigned buttons, dpad;
@@ -135,18 +174,28 @@ struct Frame {
 // ================================================================= globals
 
 static HubLink gHubs[2] = {
-    {Lpf2Hub(), HUB0_ADDR, "hub0", false, 0, 0},
-    {Lpf2Hub(), HUB1_ADDR, "hub1", false, 0, 0},
+    {Lpf2Hub(), HUB0_ADDR, "hub0", false, 0, 0, 0},
+    {Lpf2Hub(), HUB1_ADDR, "hub1", false, 0, 0, 0},
 };
 
-// Indexes: 0 rightTrack, 1 leftTrack, 2 bodyLift,
-//          3 headTilt,   4 headTurn,  5 leftArm, 6 rightArm
-static PortState gPort[7] = {{999, 0}, {999, 0}, {999, 0}, {999, 0},
-                             {999, 0}, {999, 0}, {999, 0}};
+// Indexes follow physical ports, not functions:
+//   0 hub0/A  1 hub0/B  2 hub0/D  3 hub1/B  4 hub1/C
+static PortState gPort[5] = {{999, 0}, {999, 0}, {999, 0}, {999, 0}, {999, 0}};
+
+// Arms are position controlled, so they get their own state.
+static ArmState gArmLeft  = {INT32_MIN, 0};
+static ArmState gArmRight = {INT32_MIN, 0};
+static bool gArmsZeroed = false;
 
 static unsigned long lastFrameAt = 0;
 static bool failsafeEngaged = true;
 
+// Latched state, changed on button press rather than while held.
+static int  gTrackSpeed = SPEED_SLOW;
+static bool gLed1On = false;
+static bool gLed2On = false;
+static unsigned gPrevButtons = 0;
+
 // ================================================================= helpers
 
 // Deadzone, then rescale so the remaining travel still reaches full speed.
@@ -159,6 +208,15 @@ static int stickToSpeed(int raw, int maxSpeed) {
     return sign * (int)scaled;
 }
 
+// Analog trigger, 0..1023, to a target angle between 0 and span.
+static int32_t triggerToAngle(int raw, int32_t span) {
+    if (raw <= TRIGGER_DEADZONE) return 0;
+    long travel = (long)raw - TRIGGER_DEADZONE;
+    long full = 1023L - TRIGGER_DEADZONE;
+    if (travel > full) travel = full;
+    return (int32_t)((travel * span) / full);
+}
+
 static void driveMotor(HubLink &hl, byte port, int speed, PortState &st) {
     if (!hl.hub.isConnected()) return;
 
@@ -166,8 +224,8 @@ static void driveMotor(HubLink &hl, byte port, int speed, PortState &st) {
     bool stopping = (speed == 0 && st.lastSpeed != 0);
 
     // Stops always go out immediately. Everything else is rate limited twice:
-    // per port, and per hub - hub 1 has four motors on it, and the per-port
-    // limit alone lets through more than the hub will swallow.
+    // per port, and per hub - the per-port limit alone lets through more than
+    // a hub with several motors on it will swallow.
     if (!stopping) {
         if (speed == st.lastSpeed) return;
         if ((now - st.lastSentAt) < MOTOR_MIN_GAP_MS) return;
@@ -189,18 +247,56 @@ static void driveMotor(HubLink &hl, byte port, int speed, PortState &st) {
     hl.lastCmdAt = now;
 }
 
+// Position control. HOLD keeps the motor actively at the target rather than
+// letting gravity drag the arm back down.
+static void driveArm(HubLink &hl, byte port, int32_t target, ArmState &st) {
+    if (!hl.hub.isConnected() || !gArmsZeroed) return;
+
+    unsigned long now = millis();
+    if (labs((long)target - (long)st.lastTarget) < ARM_STEP_DEG) return;
+    if ((now - st.lastSentAt) < ARM_MIN_GAP_MS) return;
+    if ((now - hl.lastCmdAt) < HUB_MIN_GAP_MS) return;
+
+#if DEBUG_MOTORS
+    Serial.printf("TX %s port %u angle %ld\n", hl.label, port, (long)target);
+#endif
+
+    hl.hub.setAbsoluteMotorPosition(port, ARM_SPEED, target, ARM_MAX_POWER,
+                                    BrakingStyle::HOLD);
+
+    st.lastTarget = target;
+    st.lastSentAt = now;
+    hl.lastCmdAt = now;
+}
+
 static void stopEverything() {
     driveMotor(gHubs[0], PORT_A, 0, gPort[0]);
     driveMotor(gHubs[0], PORT_B, 0, gPort[1]);
     driveMotor(gHubs[0], PORT_D, 0, gPort[2]);
-    driveMotor(gHubs[1], PORT_A, 0, gPort[3]);
-    driveMotor(gHubs[1], PORT_B, 0, gPort[4]);
-    driveMotor(gHubs[1], PORT_C, 0, gPort[5]);
-    driveMotor(gHubs[1], PORT_D, 0, gPort[6]);
+    driveMotor(gHubs[1], PORT_B, 0, gPort[3]);
+    driveMotor(gHubs[1], PORT_C, 0, gPort[4]);
+
+    // Arms: a plain speed command overrides the position hold and goes limp.
+    // Reset the cached targets so the next trigger movement re-commands.
+    if (gHubs[1].hub.isConnected()) {
+        gHubs[1].hub.setTachoMotorSpeed(PORT_A, 0);
+        gHubs[1].hub.setTachoMotorSpeed(PORT_D, 0);
+    }
+    gArmLeft.lastTarget = INT32_MIN;
+    gArmRight.lastTarget = INT32_MIN;
 }
 
 // Connect the hubs one at a time. Kicking off two scans at once upsets the
 // shared NimBLE scanner and you end up with one hub connected and one sulking.
+//
+// Note the 'initialised' one-shot. init() starts an ASYNCHRONOUS scan, so
+// immediately afterwards isConnected() and isConnecting() are both still
+// false. Guarding on those alone re-enters NimBLEDevice::init() thousands of
+// times a second and the Bluetooth controller aborts.
+//
+// The scanExpiresAt deadline exists because a scan that finds nothing just
+// ends quietly - isConnecting() never goes true, so without a timeout the hub
+// sits on a dead scan until the board is power cycled.
 static void serviceHub(HubLink &hl) {
     if (hl.hub.isConnected()) return;
 
@@ -217,13 +313,40 @@ static void serviceHub(HubLink &hl) {
         return;
     }
 
+    if (hl.initialised && millis() >= hl.scanExpiresAt) {
+        Serial.printf("[%s] scan timed out, restarting\n", hl.label);
+        hl.initialised = false;
+        hl.retryAt = millis() + RECONNECT_GAP_MS;
+        return;
+    }
+
     if (!hl.initialised && millis() >= hl.retryAt) {
         Serial.printf("[%s] scanning for %s\n", hl.label, hl.addr);
         hl.hub.init(std::string(hl.addr));
         hl.initialised = true;
+        hl.scanExpiresAt = millis() + SCAN_TIMEOUT_MS;
     }
 }
 
+// Define "arms down" as angle zero. Runs once per boot, after hub1 connects,
+// with the arms physically at the bottom of their travel. Not repeated on a
+// reconnect: the hub keeps its encoder preset, and re-zeroing mid-session
+// would redefine zero wherever the arms happened to be sitting.
+static void zeroArms() {
+    if (gArmsZeroed || !gHubs[1].hub.isConnected()) return;
+
+    delay(500);   // let the hub finish reporting its ports
+    gHubs[1].hub.setAbsoluteMotorEncoderPosition(PORT_A, 0);
+    delay(200);
+    gHubs[1].hub.setAbsoluteMotorEncoderPosition(PORT_D, 0);
+    delay(200);
+
+    gArmsZeroed = true;
+    gArmLeft.lastTarget = INT32_MIN;
+    gArmRight.lastTarget = INT32_MIN;
+    Serial.println("Arms zeroed at current position");
+}
+
 static uint8_t xorChecksum(const char *s, size_t len) {
     uint8_t c = 0;
     for (size_t i = 0; i < len; i++) c ^= (uint8_t)s[i];
@@ -244,44 +367,68 @@ static bool parseFrame(char *line, Frame &f) {
                   &f.buttons, &f.dpad, &f.l2, &f.r2) == 8;
 }
 
+// Latching controls fire once per press, not continuously while held. Frames
+// arrive at ~50 Hz, so without edge detection a single press would toggle
+// twenty times.
+static void handleLatchingButtons(unsigned buttons) {
+    unsigned pressed = buttons & ~gPrevButtons;
+    gPrevButtons = buttons;
+
+    if (pressed & BTN_A) {          // Cross - alternate track speed
+        gTrackSpeed = (gTrackSpeed == SPEED_FAST) ? SPEED_SLOW : SPEED_FAST;
+        Serial.printf("track speed %d%%\n", gTrackSpeed);
+    }
+    if (pressed & BTN_B) {          // Circle - LED pair 1
+        gLed1On = !gLed1On;
+        digitalWrite(LED_1_PIN, gLed1On ? HIGH : LOW);
+    }
+    if (pressed & BTN_Y) {          // Triangle - LED pair 2
+        gLed2On = !gLed2On;
+        digitalWrite(LED_2_PIN, gLed2On ? HIGH : LOW);
+    }
+}
+
 // Tank drive. One input per motor - nothing is mixed.
 static void applyFrame(const Frame &f) {
-    // Both shoulders together is the panic stop.
-    if ((f.buttons & BTN_L1) && (f.buttons & BTN_R1)) {
+    handleLatchingButtons(f.buttons);
+
+    // Square is the panic stop. LEDs are left alone - they are not motion.
+    if (f.buttons & BTN_X) {
         stopEverything();
         return;
     }
 
-    int scale = SCALE_NORMAL;
-    if (f.buttons & BTN_L1) scale = SCALE_PRECISION;
-    if (f.buttons & BTN_R1) scale = SCALE_FULL;
+    int leftTrack  = stickToSpeed(-f.ly, TRACK_MAX) * gTrackSpeed / 100
+                     * DIR_LEFT_TRACK;
+    int rightTrack = stickToSpeed(-f.ry, TRACK_MAX) * gTrackSpeed / 100
+                     * DIR_RIGHT_TRACK;
 
-    int leftTrack  = stickToSpeed(-f.ly, TRACK_MAX) * scale / 100 * DIR_LEFT_TRACK;
-    int rightTrack = stickToSpeed(-f.ry, TRACK_MAX) * scale / 100 * DIR_RIGHT_TRACK;
+    // L1 / R1 turn the head while held.
+    int headTurn = ((f.buttons & BTN_R1) ? HEAD_MAX
+                 :  (f.buttons & BTN_L1) ? -HEAD_MAX : 0) * DIR_HEAD_TURN;
 
-    int headTilt = ((f.dpad & DPAD_U) ? HEAD_MAX
-                 :  (f.dpad & DPAD_D) ? -HEAD_MAX : 0) * DIR_HEAD_TILT;
-    int headTurn = ((f.dpad & DPAD_R) ? HEAD_MAX
-                 :  (f.dpad & DPAD_L) ? -HEAD_MAX : 0) * DIR_HEAD_TURN;
+    // D-pad: up/down lifts the body, left/right tilts the head.
+    int bodyLift = ((f.dpad & DPAD_U) ? LIFT_MAX
+                 :  (f.dpad & DPAD_D) ? -LIFT_MAX : 0) * DIR_BODY_LIFT;
+    int headTilt = ((f.dpad & DPAD_R) ? HEAD_MAX
+                 :  (f.dpad & DPAD_L) ? -HEAD_MAX : 0) * DIR_HEAD_TILT;
 
-    // R2 raises, L2 lowers. Both analog 0..1023, so the lift stays proportional.
-    int bodyLift = constrain((f.r2 - f.l2) * LIFT_MAX / 1023,
-                             -LIFT_MAX, LIFT_MAX) * DIR_BODY_LIFT;
-
-    // Square raises the left arm, Circle lowers it.
-    int leftArm  = ((f.buttons & BTN_X) ? ARM_MAX
-                 :  (f.buttons & BTN_B) ? -ARM_MAX : 0) * DIR_LEFT_ARM;
-    // Cross raises the right arm, Triangle lowers it.
-    int rightArm = ((f.buttons & BTN_A) ? ARM_MAX
-                 :  (f.buttons & BTN_Y) ? -ARM_MAX : 0) * DIR_RIGHT_ARM;
+    // Arms: trigger position IS arm angle. Released means "go to zero", which
+    // gravity is already doing, so the motor mostly just catches it.
+    int32_t leftTarget  = triggerToAngle(f.l2, ARM_SPAN_LEFT);
+    int32_t rightTarget = triggerToAngle(f.r2, ARM_SPAN_RIGHT);
 
     driveMotor(gHubs[0], PORT_A, rightTrack, gPort[0]);
     driveMotor(gHubs[0], PORT_B, leftTrack,  gPort[1]);
     driveMotor(gHubs[0], PORT_D, bodyLift,   gPort[2]);
-    driveMotor(gHubs[1], PORT_A, headTilt,   gPort[3]);
-    driveMotor(gHubs[1], PORT_B, headTurn,   gPort[4]);
-    driveMotor(gHubs[1], PORT_C, leftArm,    gPort[5]);
-    driveMotor(gHubs[1], PORT_D, rightArm,   gPort[6]);
+
+    // hub1 B is the tilt motor and C is the turn motor - the reverse of what
+    // the first build assumed.
+    driveMotor(gHubs[1], PORT_B, headTilt,   gPort[3]);
+    driveMotor(gHubs[1], PORT_C, headTurn,   gPort[4]);
+
+    driveArm(gHubs[1], PORT_A, leftTarget,  gArmLeft);
+    driveArm(gHubs[1], PORT_D, rightTarget, gArmRight);
 }
 
 // ==================================================================== main
@@ -293,7 +440,13 @@ void setup() {
     pinMode(STATUS_LED_PIN, OUTPUT);
     digitalWrite(STATUS_LED_PIN, LOW);
 
-    Serial.println("LEGO hub receiver starting");
+    pinMode(LED_1_PIN, OUTPUT);
+    pinMode(LED_2_PIN, OUTPUT);
+    digitalWrite(LED_1_PIN, LOW);
+    digitalWrite(LED_2_PIN, LOW);
+
+    Serial.println("LEGO hub receiver starting (Johnny 5 Evolved)");
+    Serial.println("Wake both hubs first. Both arms must be DOWN.");
 }
 
 void loop() {
@@ -301,6 +454,9 @@ void loop() {
     serviceHub(gHubs[0]);
     if (gHubs[0].hub.isConnected()) serviceHub(gHubs[1]);
 
+    // Zeroes once per boot. Deliberately not reset when the hub drops.
+    if (gHubs[1].hub.isConnected()) zeroArms();
+
     bool ready = gHubs[0].hub.isConnected() && gHubs[1].hub.isConnected();
     digitalWrite(STATUS_LED_PIN, ready ? HIGH : LOW);