pbio/control: Don't patch drivebase trajectory.

Partially fixes pybricks/support#972

CHANGELOG.md

@@ -13,6 +13,7 @@
 
 ### Fixed
 - Fixed allocator interfering with motor control when memory usage is high ([support#956]).
+- Fixed `Stop.NONE` not working properly for some drivebase geometries ([support#972]).
 
 ### Changed
 - Methods like `control.limits()` now check the user input and raise a
@@ -28,6 +29,7 @@
 [support#484]: https://github.com/pybricks/support/issues/484
 [support#989]: https://github.com/pybricks/support/issues/881
 [support#910]: https://github.com/pybricks/support/issues/910
+[support#972]: https://github.com/pybricks/support/issues/972
 [support#989]: https://github.com/pybricks/support/issues/989
 
 ## [3.3.0b2] - 2023-03-08

lib/pbio/include/pbio/control.h

@@ -179,7 +179,7 @@ pbio_dcmotor_actuation_t pbio_control_passive_completion_to_actuation_type(pbio_
 // Start new control command:
 
 pbio_error_t pbio_control_start_position_control(pbio_control_t *ctl, uint32_t time_now, const pbio_control_state_t *state, int32_t position, int32_t speed, pbio_control_on_completion_t on_completion);
-pbio_error_t pbio_control_start_position_control_relative(pbio_control_t *ctl, uint32_t time_now, const pbio_control_state_t *state, int32_t distance, int32_t speed, pbio_control_on_completion_t on_completion);
+pbio_error_t pbio_control_start_position_control_relative(pbio_control_t *ctl, uint32_t time_now, const pbio_control_state_t *state, int32_t distance, int32_t speed, pbio_control_on_completion_t on_completion, bool allow_trajectory_shift);
 pbio_error_t pbio_control_start_position_control_hold(pbio_control_t *ctl, uint32_t time_now, int32_t position);
 pbio_error_t pbio_control_start_timed_control(pbio_control_t *ctl, uint32_t time_now, const pbio_control_state_t *state, uint32_t duration, int32_t speed, pbio_control_on_completion_t on_completion);
 

lib/pbio/src/control.c

@@ -464,7 +464,7 @@ void pbio_control_reset(pbio_control_t *ctl) {
     // subsequent maneuvers, so nothing else needs to be reset explicitly.
 }
 
-static pbio_error_t _pbio_control_start_position_control(pbio_control_t *ctl, uint32_t time_now, const pbio_control_state_t *state, const pbio_angle_t *target, int32_t speed, pbio_control_on_completion_t on_completion) {
+static pbio_error_t _pbio_control_start_position_control(pbio_control_t *ctl, uint32_t time_now, const pbio_control_state_t *state, const pbio_angle_t *target, int32_t speed, pbio_control_on_completion_t on_completion, bool allow_trajectory_shift) {
 
     pbio_error_t err;
 
@@ -531,7 +531,7 @@ static pbio_error_t _pbio_control_start_position_control(pbio_control_t *ctl, ui
         // better than just branching off. Instead, we can adjust the command
         // so it starts from the same point as the previous trajectory. This
         // avoids rounding errors when restarting commands in a tight loop.
-        if (ctl->trajectory.a0 == ref.acceleration) {
+        if (ctl->trajectory.a0 == ref.acceleration && allow_trajectory_shift) {
 
             // Update command with shifted starting point, equal to ongoing
             // maneuver.
@@ -573,7 +573,7 @@ pbio_error_t pbio_control_start_position_control(pbio_control_t *ctl, uint32_t t
     pbio_control_settings_app_to_ctl_long(&ctl->settings, position, &target);
 
     // Start position control in control units.
-    return _pbio_control_start_position_control(ctl, time_now, state, &target, pbio_control_settings_app_to_ctl(&ctl->settings, speed), on_completion);
+    return _pbio_control_start_position_control(ctl, time_now, state, &target, pbio_control_settings_app_to_ctl(&ctl->settings, speed), on_completion, true);
 }
 
 /**
@@ -584,15 +584,16 @@ pbio_error_t pbio_control_start_position_control(pbio_control_t *ctl, uint32_t t
  * This function computes what the new target position will be, and then
  * calls pbio_control_start_position_control to get there.
  *
- * @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 [in]  distance        The distance to run by (application units).
- * @param [in]  speed           The top speed on the way to the target (application units). Negative speed flips the distance sign.
- * @param [in]  on_completion   What to do when reaching the target position.
- * @return                      Error code.
+ * @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 [in]  distance               The distance to run by (application units).
+ * @param [in]  speed                  The top speed on the way to the target (application units). Negative speed flips the distance sign.
+ * @param [in]  on_completion          What to do when reaching the target position.
+ * @param [in]  allow_trajectory_shift Whether trajectory may be time-shifted for better performance in tight loops (true) or not (false).
+ * @return                             Error code.
  */
-pbio_error_t pbio_control_start_position_control_relative(pbio_control_t *ctl, uint32_t time_now, const pbio_control_state_t *state, int32_t distance, int32_t speed, pbio_control_on_completion_t on_completion) {
+pbio_error_t pbio_control_start_position_control_relative(pbio_control_t *ctl, uint32_t time_now, const pbio_control_state_t *state, int32_t distance, int32_t speed, pbio_control_on_completion_t on_completion, bool allow_trajectory_shift) {
 
     // Convert distance to control units.
     pbio_angle_t increment;
@@ -627,7 +628,7 @@ pbio_error_t pbio_control_start_position_control_relative(pbio_control_t *ctl, u
         }
     }
 
-    return _pbio_control_start_position_control(ctl, time_now, state, &target, pbio_control_settings_app_to_ctl(&ctl->settings, speed), on_completion);
+    return _pbio_control_start_position_control(ctl, time_now, state, &target, pbio_control_settings_app_to_ctl(&ctl->settings, speed), on_completion, allow_trajectory_shift);
 }
 
 /**

lib/pbio/src/drivebase.c

@@ -489,13 +489,13 @@ static pbio_error_t pbio_drivebase_drive_relative(pbio_drivebase_t *db, int32_t
     }
 
     // Start controller that controls the average angle of both motors.
-    err = pbio_control_start_position_control_relative(&db->control_distance, time_now, &state_distance, distance, drive_speed, on_completion);
+    err = pbio_control_start_position_control_relative(&db->control_distance, time_now, &state_distance, distance, drive_speed, on_completion, false);
     if (err != PBIO_SUCCESS) {
         return err;
     }
 
     // Start controller that controls half the difference between both angles.
-    err = pbio_control_start_position_control_relative(&db->control_heading, time_now, &state_heading, angle, turn_speed, on_completion);
+    err = pbio_control_start_position_control_relative(&db->control_heading, time_now, &state_heading, angle, turn_speed, on_completion, false);
     if (err != PBIO_SUCCESS) {
         return err;
     }

lib/pbio/src/servo.c

@@ -710,7 +710,7 @@ pbio_error_t pbio_servo_run_angle(pbio_servo_t *srv, int32_t speed, int32_t angl
     }
 
     // Start the relative angle control
-    return pbio_control_start_position_control_relative(&srv->control, time_now, &state, angle, speed, on_completion);
+    return pbio_control_start_position_control_relative(&srv->control, time_now, &state, angle, speed, on_completion, true);
 }
 
 /**