pbio/control: Reset completion state between program runs.

The smart coast option introduces a dependency between sequential
maneuvers, since the next starting point depends on the previous
endpoint. We don't want this dependency to persist when starting a new
program or reinitializing the device.

This is accomplished by resetting the relevant state when initializing
the controller when setting up a servo or drivebase.

Author: laurensvalk

lib/pbio/include/pbio/control.h

@@ -102,7 +102,18 @@ pbio_error_t pbio_control_settings_set_stall_tolerances(pbio_control_settings_t
 int32_t pbio_control_settings_get_max_integrator(pbio_control_settings_t *s);
 int32_t pbio_control_get_ref_time(pbio_control_t *ctl, int32_t time_now);
 
+/**
+ * Resets and initializes the control state. This is called when a device that
+ * uses this controller is first initialized or when it is disconnected.
+ */
+void pbio_control_reset(pbio_control_t *ctl);
+
+/**
+ * Stops (but not resets) the update loop from updating this controller. This
+ * is normally called when a motor coasts or brakes.
+ */
 void pbio_control_stop(pbio_control_t *ctl);
+
 pbio_error_t pbio_control_start_angle_control(pbio_control_t *ctl, int32_t time_now, pbio_control_state_t *state, int32_t target_count, int32_t target_rate, pbio_control_on_completion_t on_completion);
 pbio_error_t pbio_control_start_relative_angle_control(pbio_control_t *ctl, int32_t time_now, pbio_control_state_t *state, int32_t relative_target_count, int32_t target_rate, pbio_control_on_completion_t on_completion);
 pbio_error_t pbio_control_start_timed_control(pbio_control_t *ctl, int32_t time_now, pbio_control_state_t *state, int32_t duration, int32_t target_rate, pbio_control_on_completion_t on_completion);

lib/pbio/src/control.c

@@ -217,13 +217,26 @@ void pbio_control_update(pbio_control_t *ctl, int32_t time_now, pbio_control_sta
     pbio_logger_update(&ctl->log, log_data);
 }
 
-
+// Stops the update loop from updating this controller.
 void pbio_control_stop(pbio_control_t *ctl) {
     ctl->type = PBIO_CONTROL_NONE;
     ctl->on_target = true;
     ctl->stalled = false;
 }
 
+// Initializes the control state when creating the control object.
+void pbio_control_reset(pbio_control_t *ctl) {
+
+    // Stop the control loop update.
+    pbio_control_stop(ctl);
+
+    // Reset the previous on-completion state.
+    ctl->on_completion = PBIO_CONTROL_ON_COMPLETION_COAST;
+
+    // The on_completion state is the only persistent setting between
+    // subsequent maneuvers, so nothing else needs to be reset explicitly.
+}
+
 pbio_error_t pbio_control_start_angle_control(pbio_control_t *ctl, int32_t time_now, pbio_control_state_t *state, int32_t target_count, int32_t target_rate, pbio_control_on_completion_t on_completion) {
 
     pbio_error_t err;
@@ -334,10 +347,6 @@ pbio_error_t pbio_control_start_relative_angle_control(pbio_control_t *ctl, int3
             // We're close enough, so make the new target relative to the
             // endpoint of the last one.
             count_start = ctl->trajectory.th3;
-
-            // FIXME: We need to clear the on-completion state between program
-            // to make each program run independent.
-
         } else {
             // No special cases apply, so the best we can do is just start from
             // the current state.

lib/pbio/src/drivebase.c

@@ -188,7 +188,8 @@ pbio_error_t pbio_drivebase_get_drivebase(pbio_drivebase_t **db_address, pbio_se
     pbio_parent_set(&right->parent, db, pbio_drivebase_stop_from_servo);
 
     // Stop any existing drivebase controls
-    pbio_drivebase_stop_drivebase_control(db);
+    pbio_control_reset(&db->control_distance);
+    pbio_control_reset(&db->control_heading);
 
     // Drivebase geometry
     if (wheel_diameter <= 0 || axle_track <= 0) {

lib/pbio/src/servo.c

@@ -113,7 +113,7 @@ void pbio_servo_update_all(void) {
                 pbio_dcmotor_coast(srv->dcmotor);
 
                 // Stop the control state.
-                pbio_control_stop(&srv->control);
+                pbio_control_reset(&srv->control);
 
                 // Stop higher level controls, such as drive bases.
                 pbio_parent_stop(&srv->parent, false);
@@ -148,7 +148,7 @@ static pbio_error_t pbio_servo_stop_from_dcmotor(void *servo, bool clear_parent)
     // electrically. All we have to do here is stop the control loop,
     // so it won't override the dcmotor to do something else.
     if (pbio_control_is_active(&srv->control)) {
-        pbio_control_stop(&srv->control);
+        pbio_control_reset(&srv->control);
 
         // If we're not clearing the parent, we are done here. We don't want
         // to keep calling the drive base stop over and over.
@@ -192,7 +192,7 @@ pbio_error_t pbio_servo_setup(pbio_servo_t *srv, pbio_direction_t direction, fix
     pbio_parent_set(&srv->dcmotor->parent, srv, pbio_servo_stop_from_dcmotor);
 
     // Reset state
-    pbio_control_stop(&srv->control);
+    pbio_control_reset(&srv->control);
 
     // Get the device type to load relevant settings.
     pbio_iodev_type_id_t type_id;