pbio/drivebase: Fix synchronized pause state.

Now, the paused controller cannot unpause itself until all controllers are unpaused.

This is simpler, and also generalizes to N motors when we go beyond drivebases.

Partially fixes pybricks/support#1032

lib/pbio/include/pbio/control.h

@@ -165,7 +165,14 @@ void pbio_control_get_reference(pbio_control_t *ctl, uint32_t time_now, const pb
 
 void pbio_control_reset(pbio_control_t *ctl);
 void pbio_control_stop(pbio_control_t *ctl);
-void pbio_control_update(pbio_control_t *ctl, uint32_t time_now, const pbio_control_state_t *state, pbio_trajectory_reference_t *ref, pbio_dcmotor_actuation_t *actuation, int32_t *control);
+void pbio_control_update(
+    pbio_control_t *ctl,
+    uint32_t time_now,
+    const pbio_control_state_t *state,
+    pbio_trajectory_reference_t *ref,
+    pbio_dcmotor_actuation_t *actuation,
+    int32_t *control,
+    bool *external_pause);
 
 // Control status checks:
 

lib/pbio/include/pbio/drivebase.h

@@ -23,6 +23,10 @@ typedef struct _pbio_drivebase_t {
      * True if a gyro or compass is used for heading control, else false.
      */
     bool use_gyro;
+    /**
+     * Synchronization state to indicate that one or more controllers are paused.
+     */
+    bool control_paused;
     pbio_servo_t *left;
     pbio_servo_t *right;
     pbio_control_t control_heading;

lib/pbio/src/control.c

@@ -216,14 +216,23 @@ static int32_t pbio_control_get_pid_kp(const pbio_control_settings_t *settings,
 /**
  * Updates the PID controller state to calculate the next actuation step.
  *
- * @param [in]  ctl             The control instance.
- * @param [in]  time_now        The wall time (ticks).
- * @param [in]  state           The current state of the system being controlled (control units).
- * @param [out] ref             Computed reference point on the trajectory (control units).
- * @param [out] actuation       Required actuation type.
- * @param [out] control         The control output, which is the actuation payload (control units).
+ * @param [in]   ctl              The control instance.
+ * @param [in]   time_now         The wall time (ticks).
+ * @param [in]   state            The current state of the system being controlled (control units).
+ * @param [out]  ref              Computed reference point on the trajectory (control units).
+ * @param [out]  actuation        Required actuation type.
+ * @param [out]  control          The control output, which is the actuation payload (control units).
+ * @param [inout] external_pause  Whether to force the controller to pause using external information (in), and
+ *                                whether the controller still needs pausing according to its own state (out).
  */
-void pbio_control_update(pbio_control_t *ctl, uint32_t time_now, const pbio_control_state_t *state, pbio_trajectory_reference_t *ref, pbio_dcmotor_actuation_t *actuation, int32_t *control) {
+void pbio_control_update(
+    pbio_control_t *ctl,
+    uint32_t time_now,
+    const pbio_control_state_t *state,
+    pbio_trajectory_reference_t *ref,
+    pbio_dcmotor_actuation_t *actuation,
+    int32_t *control,
+    bool *external_pause) {
 
     // Get reference signals at the reference time point in the trajectory.
     // This compensates for any time we may have spent pausing when the motor was stalled.
@@ -290,7 +299,7 @@ void pbio_control_update(pbio_control_t *ctl, uint32_t time_now, const pbio_cont
 
     // Position anti-windup in case of angle control (accumulated position error may not get too high)
     if (pbio_control_type_is_position(ctl)) {
-        if (pause_integration) {
+        if (pause_integration || *external_pause) {
             // We are at the torque limit and we should prevent further position error integration.
             pbio_position_integrator_pause(&ctl->position_integrator, time_now);
         } else {
@@ -300,7 +309,7 @@ void pbio_control_update(pbio_control_t *ctl, uint32_t time_now, const pbio_cont
     }
     // Position anti-windup in case of timed speed control (speed integral may not get too high)
     else {
-        if (pause_integration) {
+        if (pause_integration || *external_pause) {
             // We are at the torque limit and we should prevent further speed error integration.
             pbio_speed_integrator_pause(&ctl->speed_integrator, time_now, position_error);
         } else {
@@ -309,6 +318,9 @@ void pbio_control_update(pbio_control_t *ctl, uint32_t time_now, const pbio_cont
         }
     }
 
+    // Informs caller if pausing is still needed according to this controller's state.
+    *external_pause = pause_integration;
+
     // Check if controller is stalled, and set the status.
     pbio_control_status_set(ctl, PBIO_CONTROL_STATUS_STALLED,
         pbio_control_type_is_position(ctl) ?

lib/pbio/src/drivebase.c

@@ -164,6 +164,7 @@ static void pbio_drivebase_stop_drivebase_control(pbio_drivebase_t *db) {
     // Stop drivebase control so polling will stop
     pbio_control_stop(&db->control_distance);
     pbio_control_stop(&db->control_heading);
+    db->control_paused = false;
 }
 
 /**
@@ -291,6 +292,7 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_se
     // Stop any existing drivebase controls
     pbio_control_reset(&db->control_distance);
     pbio_control_reset(&db->control_heading);
+    db->control_paused = false;
 
     // Reset both motors to a passive state
     pbio_drivebase_stop_servo_control(db);
@@ -405,13 +407,18 @@ static pbio_error_t pbio_drivebase_update(pbio_drivebase_t *db) {
     pbio_trajectory_reference_t ref_distance;
     int32_t distance_torque;
     pbio_dcmotor_actuation_t distance_actuation;
-    pbio_control_update(&db->control_distance, time_now, &state_distance, &ref_distance, &distance_actuation, &distance_torque);
+    bool distance_external_pause = db->control_paused;
+    pbio_control_update(&db->control_distance, time_now, &state_distance, &ref_distance, &distance_actuation, &distance_torque, &distance_external_pause);
 
     // Get reference and torque signals for heading control.
     pbio_trajectory_reference_t ref_heading;
     int32_t heading_torque;
     pbio_dcmotor_actuation_t heading_actuation;
-    pbio_control_update(&db->control_heading, time_now, &state_heading, &ref_heading, &heading_actuation, &heading_torque);
+    bool heading_external_pause = db->control_paused;
+    pbio_control_update(&db->control_heading, time_now, &state_heading, &ref_heading, &heading_actuation, &heading_torque, &heading_external_pause);
+
+    // If either controller is paused, pause both.
+    db->control_paused = distance_external_pause || heading_external_pause;
 
     // If either controller coasts, coast both, thereby also stopping control.
     if (distance_actuation == PBIO_DCMOTOR_ACTUATION_COAST ||
@@ -430,19 +437,6 @@ static pbio_error_t pbio_drivebase_update(pbio_drivebase_t *db) {
         return PBIO_ERROR_FAILED;
     }
 
-    // Both controllers are able to stop the other when it stalls. This ensures
-    // they complete at exactly the same time.
-    if (pbio_control_type_is_position(&db->control_distance) &&
-        pbio_position_integrator_is_paused(&db->control_distance.position_integrator)) {
-        // If distance controller is paused, pause heading control too.
-        pbio_position_integrator_pause(&db->control_heading.position_integrator, time_now);
-    }
-    if (pbio_control_type_is_position(&db->control_heading) &&
-        pbio_position_integrator_is_paused(&db->control_heading.position_integrator)) {
-        // If heading controller is paused, pause distance control too.
-        pbio_position_integrator_pause(&db->control_distance.position_integrator, time_now);
-    }
-
     // The left servo drives at a torque and speed of (average) + (difference).
     int32_t feed_forward_left = pbio_observer_get_feedforward_torque(
         db->left->observer.model,

lib/pbio/src/servo.c

@@ -95,7 +95,8 @@ static pbio_error_t pbio_servo_update(pbio_servo_t *srv) {
 
         // Calculate feedback control signal
         pbio_dcmotor_actuation_t requested_actuation;
-        pbio_control_update(&srv->control, time_now, &state, &ref, &requested_actuation, &feedback_torque);
+        bool external_pause = false;
+        pbio_control_update(&srv->control, time_now, &state, &ref, &requested_actuation, &feedback_torque, &external_pause);
 
         // Get required feedforward torque for current reference
         feedforward_torque = pbio_observer_get_feedforward_torque(srv->observer.model, ref.speed, ref.acceleration);