pbio/drivebase: Stop updating on servo failure.

This makes behavior consistent with servos.

lib/pbio/include/pbio/drivebase.h

@@ -20,6 +20,7 @@ typedef struct _pbio_drivebase_t {
 pbio_error_t pbio_drivebase_get_drivebase(pbio_servo_t *left, pbio_servo_t *right, pbio_drivebase_t **db);
 pbio_error_t pbio_drivebase_setup(pbio_drivebase_t *db, fix16_t wheel_diameter, fix16_t axle_track);
 void pbio_drivebase_update_all(void);
+bool pbio_drivebase_update_loop_is_running(pbio_drivebase_t *db);
 
 // Finite point to point control
 

lib/pbio/src/drivebase.c

@@ -29,6 +29,23 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_servo_t *left, pbio_servo_t *righ
     return PBIO_SUCCESS;
 }
 
+// The drivebase update can run if both servos are successfully updating
+bool pbio_drivebase_update_loop_is_running(pbio_drivebase_t *db) {
+
+    // Drivebase must have servos.
+    if (!db->left || !db->right) {
+        return false;
+    }
+
+    // Drivebase must be the parent of its two servos.
+    if (db->left->parent.parent_object != db || !db->right->parent.parent_object) {
+        return false;
+    }
+
+    // Both servo update loops must be running.
+    return pbio_servo_update_loop_is_running(db->left) && pbio_servo_update_loop_is_running(db->right);
+}
+
 static pbio_error_t drivebase_adopt_settings(pbio_control_settings_t *s_distance, pbio_control_settings_t *s_heading, pbio_control_settings_t *s_left, pbio_control_settings_t *s_right) {
 
     // All rate/count acceleration limits add up, because distance state is two motors counts added
@@ -219,6 +236,11 @@ pbio_error_t pbio_drivebase_setup(pbio_drivebase_t *db, fix16_t wheel_diameter,
 
 pbio_error_t pbio_drivebase_stop(pbio_drivebase_t *db, pbio_actuation_t after_stop) {
 
+    // Don't allow new user command if update loop not registered.
+    if (!pbio_drivebase_update_loop_is_running(db)) {
+        return PBIO_ERROR_INVALID_OP;
+    }
+
     if (after_stop == PBIO_ACTUATION_HOLD) {
 
         // Get drive base state
@@ -306,11 +328,18 @@ static pbio_error_t pbio_drivebase_update(pbio_drivebase_t *db) {
 
 void pbio_drivebase_update_all(void) {
     // TODO: Allow more than one drivebase.
-    pbio_drivebase_update(&drivebases[0]);
+    if (pbio_drivebase_update_loop_is_running(&drivebases[0])) {
+        pbio_drivebase_update(&drivebases[0]);
+    }
 }
 
 static pbio_error_t pbio_drivebase_drive_counts_relative(pbio_drivebase_t *db, int32_t sum, int32_t sum_rate, int32_t dif, int32_t dif_rate, pbio_actuation_t after_stop) {
 
+    // Don't allow new user command if update loop not registered.
+    if (!pbio_drivebase_update_loop_is_running(db)) {
+        return PBIO_ERROR_INVALID_OP;
+    }
+
     // Get current time
     int32_t time_now = pbdrv_clock_get_us();
 
@@ -389,6 +418,11 @@ pbio_error_t pbio_drivebase_drive_curve(pbio_drivebase_t *db, int32_t radius, in
 
 static pbio_error_t pbio_drivebase_drive_counts_timed(pbio_drivebase_t *db, int32_t sum_rate, int32_t dif_rate, int32_t duration, pbio_control_on_target_t stop_func, pbio_actuation_t after_stop) {
 
+    // Don't allow new user command if update loop not registered.
+    if (!pbio_drivebase_update_loop_is_running(db)) {
+        return PBIO_ERROR_INVALID_OP;
+    }
+
     // Get current time
     int32_t time_now = pbdrv_clock_get_us();
 

lib/pbio/src/servo.c

@@ -114,6 +114,9 @@ void pbio_servo_update_all(void) {
 
                 // Stop the control state.
                 pbio_control_stop(&srv->control);
+
+                // Stop higher level controls, such as drive bases.
+                pbio_parent_stop(&srv->parent, false);
             }
         }
     }
@@ -300,8 +303,8 @@ pbio_error_t pbio_servo_actuate(pbio_servo_t *srv, pbio_actuation_t actuation_ty
 
 pbio_error_t pbio_servo_stop(pbio_servo_t *srv, pbio_actuation_t after_stop) {
 
-    // Don't allow new user command if control loop not registered.
-    if (!srv->run_update_loop) {
+    // Don't allow new user command if update loop not registered.
+    if (!pbio_servo_update_loop_is_running(srv)) {
         return PBIO_ERROR_INVALID_OP;
     }
 
@@ -331,8 +334,8 @@ pbio_error_t pbio_servo_stop(pbio_servo_t *srv, pbio_actuation_t after_stop) {
 
 static pbio_error_t pbio_servo_run_timed(pbio_servo_t *srv, int32_t speed, int32_t duration, pbio_control_on_target_t stop_func, pbio_actuation_t after_stop) {
 
-    // Don't allow new user command if control loop not registered.
-    if (!srv->run_update_loop) {
+    // Don't allow new user command if update loop not registered.
+    if (!pbio_servo_update_loop_is_running(srv)) {
         return PBIO_ERROR_INVALID_OP;
     }
 
@@ -381,8 +384,8 @@ pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, pbio
 
 pbio_error_t pbio_servo_run_target(pbio_servo_t *srv, int32_t speed, int32_t target, pbio_actuation_t after_stop) {
 
-    // Don't allow new user command if control loop not registered.
-    if (!srv->run_update_loop) {
+    // Don't allow new user command if update loop not registered.
+    if (!pbio_servo_update_loop_is_running(srv)) {
         return PBIO_ERROR_INVALID_OP;
     }
 
@@ -411,8 +414,8 @@ pbio_error_t pbio_servo_run_target(pbio_servo_t *srv, int32_t speed, int32_t tar
 
 pbio_error_t pbio_servo_run_angle(pbio_servo_t *srv, int32_t speed, int32_t angle, pbio_actuation_t after_stop) {
 
-    // Don't allow new user command if control loop not registered.
-    if (!srv->run_update_loop) {
+    // Don't allow new user command if update loop not registered.
+    if (!pbio_servo_update_loop_is_running(srv)) {
         return PBIO_ERROR_INVALID_OP;
     }
 
@@ -442,8 +445,8 @@ pbio_error_t pbio_servo_run_angle(pbio_servo_t *srv, int32_t speed, int32_t angl
 
 pbio_error_t pbio_servo_track_target(pbio_servo_t *srv, int32_t target) {
 
-    // Don't allow new user command if control loop not registered.
-    if (!srv->run_update_loop) {
+    // Don't allow new user command if update loop not registered.
+    if (!pbio_servo_update_loop_is_running(srv)) {
         return PBIO_ERROR_INVALID_OP;
     }
 

pybricks/robotics/pb_type_drivebase.c

@@ -93,6 +93,9 @@ STATIC void wait_for_completion_drivebase(pbio_drivebase_t *db) {
     while (pbio_drivebase_is_busy(db)) {
         mp_hal_delay_ms(5);
     }
+    if (!pbio_drivebase_update_loop_is_running(db)) {
+        pb_assert(PBIO_ERROR_NO_DEV);
+    }
 }
 
 // pybricks.robotics.DriveBase.straight

pybricks/robotics/pb_type_spikebase.c

@@ -65,6 +65,9 @@ STATIC void wait_for_completion_drivebase(pbio_drivebase_t *db) {
     while (pbio_drivebase_is_busy(db)) {
         mp_hal_delay_ms(5);
     }
+    if (!pbio_drivebase_update_loop_is_running(db)) {
+        pb_assert(PBIO_ERROR_NO_DEV);
+    }
 }
 
 // pybricks.robotics.SpikeBase.tank_move_for_degrees