pbio/control: Implement smart coast.

Added `Stop.COAST_SMART` as `then` option for motors. This still coasts the
motor, but it keeps track of the previously used position target. When a new
relative angle command is given (e.g. rotate 90 degrees), it uses that
position as the starting point. This avoids accumulation of errors when using
relative angles in succession

Author: laurensvalk

CHANGELOG.md

@@ -7,6 +7,11 @@
 ### Added
 - Added `Stop.NONE` as `then` option for motors. This allows subsequent
   motor and drive base commands to transition without stopping.
+- Added `Stop.COAST_SMART` as `then` option for motors. This still coasts the
+  motor, but it keeps track of the previously used position target. When a new
+  relative angle command is given (e.g. rotate 90 degrees), it uses that
+  position as the starting point. This avoids accumulation of errors when using
+  relative angles in succession.
 - Made motor deceleration configurable separately from acceleration.
 - Enabled `ujson` module.
 - Added ability to use more than one `DriveBase` in the same script.

lib/pbio/include/pbio/control.h

@@ -51,6 +51,9 @@ typedef enum {
     PBIO_CONTROL_ON_COMPLETION_HOLD,
     /** On completion, keep moving at target speed */
     PBIO_CONTROL_ON_COMPLETION_CONTINUE,
+    /** On completion, coast the motor, but use endpoint of this maneuver
+     * as starting point for the next relative angle maneuver. */
+    PBIO_CONTROL_ON_COMPLETION_COAST_SMART,
 } pbio_control_on_completion_t;
 
 // State of a system being controlled.

lib/pbio/src/control.c

@@ -154,15 +154,32 @@ void pbio_control_update(pbio_control_t *ctl, int32_t time_now, pbio_control_sta
         // If on target, decide what to do next using the on-completion type.
         switch (ctl->on_completion) {
             case PBIO_CONTROL_ON_COMPLETION_COAST:
+                // Coast the motor and stop the control loop.
                 *actuation = PBIO_DCMOTOR_ACTUATION_COAST;
                 *control = 0;
                 pbio_control_stop(ctl);
                 break;
             case PBIO_CONTROL_ON_COMPLETION_BRAKE:
+                // Passively brake and stop the control loop.
                 *actuation = PBIO_DCMOTOR_ACTUATION_BRAKE;
                 *control = 0;
                 pbio_control_stop(ctl);
                 break;
+            case PBIO_CONTROL_ON_COMPLETION_COAST_SMART:
+                // For smart coast, keep actuating (holding) briefly to enforce
+                // standstill. It also gives some time for two subsequent
+                // blocks to smoothly transition without going through coast.
+                if (time_ref - ctl->trajectory.t3 < 100 * US_PER_MS) {
+                    *actuation = PBIO_DCMOTOR_ACTUATION_TORQUE;
+                    *control = torque;
+                }
+                // After that, coast the motor and stop the control loop.
+                else {
+                    *actuation = PBIO_DCMOTOR_ACTUATION_COAST;
+                    *control = 0;
+                    pbio_control_stop(ctl);
+                }
+                break;
             case PBIO_CONTROL_ON_COMPLETION_CONTINUE:
             // Fall through, same as hold.
             case PBIO_CONTROL_ON_COMPLETION_HOLD:
@@ -298,19 +315,34 @@ pbio_error_t pbio_control_start_angle_control(pbio_control_t *ctl, int32_t time_
 
 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) {
 
-    // Get the count from which the relative count is to be counted
+    // First, we need to decide where the relative motion starts from.
     int32_t count_start;
 
-    // If no control is active, count from the physical count
-    if (!pbio_control_is_active(ctl)) {
-        count_start = state->count;
-    }
-    // Otherwise count from the current reference
-    else {
+    if (pbio_control_is_active(ctl)) {
+        // If control is already active, restart from current reference.
         int32_t time_ref = pbio_control_get_ref_time(ctl, time_now);
         pbio_trajectory_reference_t ref;
         pbio_trajectory_get_reference(&ctl->trajectory, time_ref, &ref);
         count_start = ref.count;
+    } else {
+        // Control is inactive. We still have two options.
+        // If the previous command used smart coast and we're still close to
+        // its target, we want to start from there. This avoids accumulating
+        // errors in programs that use mostly relative motions like run_angle.
+        if (ctl->on_completion == PBIO_CONTROL_ON_COMPLETION_COAST_SMART &&
+            abs(ctl->trajectory.th3 - state->count) < ctl->settings.count_tolerance * 2) {
+            // 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.
+            count_start = state->count;
+        }
     }
 
     // The target count is the start count plus the count to be traveled.  If speed is negative, traveled count also flips.
@@ -360,6 +392,11 @@ pbio_error_t pbio_control_start_timed_control(pbio_control_t *ctl, int32_t time_
 
     pbio_error_t err;
 
+    if (on_completion == PBIO_CONTROL_ON_COMPLETION_COAST_SMART) {
+        // For timed maneuvers, the end point has no meaning, so just coast.
+        on_completion = PBIO_CONTROL_ON_COMPLETION_COAST;
+    }
+
     // Set new maneuver action and stop type, and state
     ctl->on_completion = on_completion;
     ctl->on_target = false;

lib/pbio/src/servo.c

@@ -333,8 +333,9 @@ pbio_error_t pbio_servo_stop(pbio_servo_t *srv, pbio_control_on_completion_t on_
         return err;
     }
 
-    switch (on_completion)
-    {
+    switch (on_completion) {
+        case PBIO_CONTROL_ON_COMPLETION_COAST_SMART:
+        // Same as normal coast, so fall through.
         case PBIO_CONTROL_ON_COMPLETION_COAST:
             return pbio_servo_actuate(srv, PBIO_DCMOTOR_ACTUATION_COAST, 0);
         case PBIO_CONTROL_ON_COMPLETION_BRAKE:

pybricks/parameters/pb_type_stop.c

@@ -17,6 +17,12 @@ const pb_obj_enum_member_t pb_Stop_COAST_obj = {
     .value = PBIO_CONTROL_ON_COMPLETION_COAST
 };
 
+const pb_obj_enum_member_t pb_Stop_COAST_SMART_obj = {
+    {&pb_enum_type_Stop},
+    .name = MP_QSTR_COAST_SMART,
+    .value = PBIO_CONTROL_ON_COMPLETION_COAST_SMART,
+};
+
 const pb_obj_enum_member_t pb_Stop_BRAKE_obj = {
     {&pb_enum_type_Stop},
     .name = MP_QSTR_BRAKE,
@@ -36,10 +42,11 @@ const pb_obj_enum_member_t pb_Stop_NONE_obj = {
 };
 
 STATIC const mp_rom_map_elem_t pb_enum_Stop_table[] = {
-    { MP_ROM_QSTR(MP_QSTR_COAST),   MP_ROM_PTR(&pb_Stop_COAST_obj) },
-    { MP_ROM_QSTR(MP_QSTR_BRAKE),   MP_ROM_PTR(&pb_Stop_BRAKE_obj) },
-    { MP_ROM_QSTR(MP_QSTR_HOLD),    MP_ROM_PTR(&pb_Stop_HOLD_obj)  },
-    { MP_ROM_QSTR(MP_QSTR_NONE),    MP_ROM_PTR(&pb_Stop_NONE_obj)},
+    { MP_ROM_QSTR(MP_QSTR_COAST),       MP_ROM_PTR(&pb_Stop_COAST_obj) },
+    { MP_ROM_QSTR(MP_QSTR_COAST_SMART), MP_ROM_PTR(&pb_Stop_COAST_SMART_obj) },
+    { MP_ROM_QSTR(MP_QSTR_BRAKE),       MP_ROM_PTR(&pb_Stop_BRAKE_obj) },
+    { MP_ROM_QSTR(MP_QSTR_HOLD),        MP_ROM_PTR(&pb_Stop_HOLD_obj)  },
+    { MP_ROM_QSTR(MP_QSTR_NONE),        MP_ROM_PTR(&pb_Stop_NONE_obj)},
 };
 STATIC MP_DEFINE_CONST_DICT(pb_enum_type_Stop_locals_dict, pb_enum_Stop_table);