pbio/port_lump: Retry mode switch on timeout.

We trigger a mode change for motors on boot, but in rare cases the motor never changes mode. Retrying works.

Author: laurensvalk

CHANGELOG.md

@@ -37,6 +37,7 @@
   connection. All of the above Remote and Xbox Controller were introduced in
   the previous beta release, so did not affect any stable release ([support#2521]).
 - Fixed Xbox Controller sometimes not working the first time ([support#1509]).
+- Fixed motors sometimes not working after boot.
 
 [support#1509]: https://github.com/pybricks/support/issues/1509
 [support#1962]: https://github.com/pybricks/support/issues/1962

lib/pbio/src/port_lump.c

@@ -1084,6 +1084,10 @@ pbio_error_t pbio_port_lump_is_ready(pbio_port_lump_dev_t *lump_dev) {
 
     // Not ready if waiting for mode change
     if (lump_dev->mode != lump_dev->mode_switch.desired_mode) {
+        if (time - lump_dev->mode_switch.time > 500) {
+            debug_pr("Mode switch timed out. Retrying.\n");
+            pbio_port_lump_request_mode(lump_dev, lump_dev->mode_switch.desired_mode);
+        }
         return PBIO_ERROR_AGAIN;
     }
 
@@ -1113,6 +1117,8 @@ pbio_error_t pbio_port_lump_is_ready(pbio_port_lump_dev_t *lump_dev) {
  */
 pbio_error_t pbio_port_lump_set_mode(pbio_port_lump_dev_t *lump_dev, uint8_t mode) {
 
+    debug_pr("Set mode to %d\n.", mode);
+
     if (!lump_dev) {
         return PBIO_ERROR_NO_DEV;
     }