From 019ed1e937641a5a2c69a9c0859b2ff71ae38bbc Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 22 May 2023 15:36:58 +0200 Subject: [PATCH] pybricks.common.Motor: Restore duty_limit behavior. This is an alternate way to use temporary limits in pbio (no nlr buf) but without a breaking change in the definition of duty limit or stall. See https://github.com/pybricks/support/issues/1069 --- CHANGELOG.md | 2 -- lib/pbio/include/pbio/servo.h | 2 +- lib/pbio/src/servo.c | 17 ++++++++++++----- pybricks/common/pb_type_motor.c | 17 +++++++---------- 4 files changed, 20 insertions(+), 18 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 541b7f2fd..717ef5a74 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,8 +20,6 @@ ### Changed - Updated MicroPython to v1.20.0. -- Changed `duty_limit` argument of `Motor.run_until_stalled` to `torque_limit`. - The original name continues to work as well. ### Fixed - Fixed stdin containing `0x06` command byte ([support#1052]). diff --git a/lib/pbio/include/pbio/servo.h b/lib/pbio/include/pbio/servo.h index 497801554..cb73ee997 100644 --- a/lib/pbio/include/pbio/servo.h +++ b/lib/pbio/include/pbio/servo.h @@ -161,7 +161,7 @@ pbio_error_t pbio_servo_stop(pbio_servo_t *srv, pbio_control_on_completion_t on_ pbio_error_t pbio_servo_reset_angle(pbio_servo_t *srv, int32_t reset_angle, bool reset_to_abs); pbio_error_t pbio_servo_run_forever(pbio_servo_t *srv, int32_t speed); pbio_error_t pbio_servo_run_time(pbio_servo_t *srv, int32_t speed, uint32_t duration, pbio_control_on_completion_t on_completion); -pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, int32_t torque_limit_pct, pbio_control_on_completion_t on_completion); +pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, int32_t torque_limit, pbio_control_on_completion_t on_completion); pbio_error_t pbio_servo_run_angle(pbio_servo_t *srv, int32_t speed, int32_t angle, pbio_control_on_completion_t on_completion); pbio_error_t pbio_servo_run_target(pbio_servo_t *srv, int32_t speed, int32_t target, pbio_control_on_completion_t on_completion); pbio_error_t pbio_servo_track_target(pbio_servo_t *srv, int32_t target); diff --git a/lib/pbio/src/servo.c b/lib/pbio/src/servo.c index 2cdf7be98..ea7a2a86f 100644 --- a/lib/pbio/src/servo.c +++ b/lib/pbio/src/servo.c @@ -101,8 +101,16 @@ static pbio_error_t pbio_servo_update(pbio_servo_t *srv) { // Get required feedforward torque for current reference feedforward_torque = pbio_observer_get_feedforward_torque(srv->observer.model, ref.speed, ref.acceleration); + // HACK: Constrain total torque to respect temporary duty_cycle limit. + // See https://github.com/pybricks/support/issues/1069. + // The feedback torque is already constrained by the temporary limit, + // and this would be enough in a run_until_overload-like scenario. But + // for now we must limit the feedforward torque also, or it would never + // get into the stall state at high speeds. + int32_t total_torque = pbio_int_math_clamp(feedback_torque + feedforward_torque, srv->control.settings.actuation_max_temporary); + // Actuate the servo. For torque control, the torque payload is passed along. Otherwise payload is ignored. - err = pbio_servo_actuate(srv, requested_actuation, feedback_torque + feedforward_torque); + err = pbio_servo_actuate(srv, requested_actuation, total_torque); if (err != PBIO_SUCCESS) { return err; } @@ -647,12 +655,11 @@ pbio_error_t pbio_servo_run_time(pbio_servo_t *srv, int32_t speed, uint32_t dura * * @param [in] srv The servo instance. * @param [in] speed Angular velocity in degrees per second. - * @param [in] torque_limit_pct Percentage of maximum feedback torque to use. - * It will still use the default feedforward torque. + * @param [in] torque_limit Maximum torque to use. * @param [in] on_completion What to do once stalled. * @return Error code. */ -pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, int32_t torque_limit_pct, pbio_control_on_completion_t on_completion) { +pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, int32_t torque_limit, pbio_control_on_completion_t on_completion) { if (on_completion == PBIO_CONTROL_ON_COMPLETION_CONTINUE) { // Can't continue after stall. @@ -673,7 +680,7 @@ pbio_error_t pbio_servo_run_until_stalled(pbio_servo_t *srv, int32_t speed, int3 srv->control.type |= PBIO_CONTROL_TYPE_FLAG_OBJECTIVE_IS_STALL; // Set the temporary torque limit used during this maneuver. - srv->control.settings.actuation_max_temporary = srv->control.settings.actuation_max * torque_limit_pct / 100; + srv->control.settings.actuation_max_temporary = torque_limit; return PBIO_SUCCESS; } diff --git a/pybricks/common/pb_type_motor.c b/pybricks/common/pb_type_motor.c index 1f885bc79..7900490c8 100644 --- a/pybricks/common/pb_type_motor.c +++ b/pybricks/common/pb_type_motor.c @@ -266,25 +266,22 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po common_Motor_obj_t, self, PB_ARG_REQUIRED(speed), PB_ARG_DEFAULT_OBJ(then, pb_Stop_COAST_obj), - PB_ARG_DEFAULT_NONE(torque_limit), PB_ARG_DEFAULT_NONE(duty_limit)); mp_int_t speed = pb_obj_get_int(speed_in); pbio_control_on_completion_t then = pb_type_enum_get_value(then_in, &pb_enum_type_Stop); - // Backwards compatibility <= v3.2: allow duty_limit arg as torque_limit. + // REVISIT: Use torque limit. See https://github.com/pybricks/support/issues/1069. + int32_t torque_limit; if (duty_limit_in != mp_const_none) { - torque_limit_in = duty_limit_in; - } - - // Get torque limit as percentage of max torque. - int32_t torque_limit_pct = 100; - if (torque_limit_in != mp_const_none) { - torque_limit_pct = pb_obj_get_pct(torque_limit_in); + int32_t voltage_limit = pbio_battery_get_voltage_from_duty_pct(pb_obj_get_pct(duty_limit_in)); + torque_limit = pbio_observer_voltage_to_torque(self->srv->observer.model, voltage_limit); + } else { + torque_limit = self->srv->control.settings.actuation_max; } // Start moving. - pb_assert(pbio_servo_run_until_stalled(self->srv, speed, torque_limit_pct, then)); + pb_assert(pbio_servo_run_until_stalled(self->srv, speed, torque_limit, then)); // Handle completion by awaiting or blocking. return pb_type_awaitable_await_or_wait(