pbio/servo: hold if reset_angle while moving
If reset_angle is called while the motor is moving, that means the user is
resetting the angle while a background maneuver is ongoing. To avoid
undefined behavior about where it should go, we stop and hold the motor.
@@ -198,8 +198,16 @@ pbio_error_t pbio_servo_reset_angle(pbio_servo_t *srv, int32_t reset_angle, bool
return PBIO_ERROR_INVALID_OP;
}
- // Perform angle reset in case of tracking / holding
- if (srv->control.type == PBIO_CONTROL_ANGLE && srv->control.on_target) {
+ // If the motor was in a passive mode (coast, brake, user duty),
+ // just reset angle and leave motor state unchanged.
+ if (srv->control.type == PBIO_CONTROL_NONE) {
+ return pbio_tacho_reset_angle(srv->tacho, reset_angle, reset_to_abs);
+ }
+
+ // If are were busy moving, that means the reset was called while a motor
+ // was running in the background. To avoid confusion as to where the motor
+ // must go after the reset, we'll make it stop and hold right here right now.
+
// Get the old angle
int32_t angle_old;
err = pbio_tacho_get_angle(srv->tacho, &angle_old);
@@ -207,7 +215,7 @@ pbio_error_t pbio_servo_reset_angle(pbio_servo_t *srv, int32_t reset_angle, bool
return err;
}
- // Get the old target angle
+ // Get the old target angle that we were tracking until now
int32_t time_ref = pbio_control_get_ref_time(&srv->control, clock_usecs());
int32_t count_ref, unused;
pbio_trajectory_get_reference(&srv->control.trajectory, time_ref, &count_ref, &unused, &unused, &unused);
@@ -218,23 +226,11 @@ pbio_error_t pbio_servo_reset_angle(pbio_servo_t *srv, int32_t reset_angle, bool
if (err != PBIO_SUCCESS) {
return err;
}
+
// Set the new target based on the old angle and the old target, after the angle reset
int32_t new_target = reset_angle + target_old - angle_old;
return pbio_servo_track_target(srv, new_target);
}
- // If the motor was in a passive mode (coast, brake, user duty), reset angle and leave state unchanged
- else if (srv->control.type == PBIO_CONTROL_NONE) {
- return pbio_tacho_reset_angle(srv->tacho, reset_angle, reset_to_abs);
- }
- // In all other cases, stop the ongoing maneuver by coasting and then reset the angle
- else {
- err = pbio_servo_stop(srv, PBIO_ACTUATION_COAST);
- if (err != PBIO_SUCCESS) {
- return err;
- }
- return pbio_tacho_reset_angle(srv->tacho, reset_angle, reset_to_abs);
- }
-}
// Get the physical state of a single motor
static pbio_error_t servo_get_state(pbio_servo_t *srv, int32_t *time_now, int32_t *count_now, int32_t *rate_now) {
