pbio/servo: fix user duty not working
pbio_servo_actuate is not supposed to be called when control is not active. This adds an explicit workaround for it, but it would be better to refactor the actuation/control so that no such check is required.
@@ -186,15 +186,17 @@ pbio_error_t pbio_servo_control_update(pbio_servo_t *srv) {
}
// Modify the feedback signal with additional feed forward terms
- if (srv->control.settings.use_estimated_speed && actuation_now == PBIO_ACTUATION_DUTY) {
+ if (srv->control.settings.use_estimated_speed && srv->control.type != PBIO_CONTROL_NONE) {
control_now += pbio_observer_get_feed_forward(&srv->observer, acceleration_ref, battery_voltage);
}
- // Apply the control type and signal
+ // FIXME: Shouldn't need this check here, so refactor duty/torque application.
+ if (srv->control.type != PBIO_CONTROL_NONE) {
err = pbio_servo_actuate(srv, actuation_now, control_now);
if (err != PBIO_SUCCESS) {
return err;
}
+ }
// Update the state observer
pbio_observer_update(&srv->observer, count_now, actuation_now, control_now, battery_voltage);
