pbio/control: distinguish actuation limits

Distinguish between the maximum PID control value and the maximum duty cycle value applied to the motors.

Author: laurensvalk

lib/pbio/include/pbio/control.h

@@ -36,7 +36,8 @@ typedef struct _pbio_control_settings_t {
     int32_t pid_kp;                 /**< Proportional position control constant (and integral speed control constant) */
     int32_t pid_ki;                 /**< Integral position control constant */
     int32_t pid_kd;                 /**< Derivative position control constant (and proportional speed control constant) */
-    int32_t max_control;            /**< Upper limit on control output */
+    int32_t max_torque;             /**< Upper limit on control torque */
+    int32_t max_duty;               /**< Upper limit on duty cycle */
     int32_t integral_range;         /**< Region around the target count in which integral errors are accumulated */
     int32_t integral_rate;          /**< Maximum rate at which the integrator is allowed to increase */
     bool use_estimated_rate;        /**< Whether to use the estimated speed (true) or the reported/measured speed (false) for feedback control */
@@ -88,8 +89,8 @@ typedef struct _pbio_control_t {
 int32_t pbio_control_counts_to_user(pbio_control_settings_t *s, int32_t counts);
 int32_t pbio_control_user_to_counts(pbio_control_settings_t *s, int32_t user);
 
-void pbio_control_settings_get_limits(pbio_control_settings_t *s, int32_t *speed, int32_t *acceleration, int32_t *actuation);
-pbio_error_t pbio_control_settings_set_limits(pbio_control_settings_t *ctl, int32_t speed, int32_t acceleration, int32_t actuation);
+void pbio_control_settings_get_limits(pbio_control_settings_t *s, int32_t *speed, int32_t *acceleration, int32_t *duty, int32_t *torque);
+pbio_error_t pbio_control_settings_set_limits(pbio_control_settings_t *ctl, int32_t speed, int32_t acceleration, int32_t duty, int32_t torque);
 
 void pbio_control_settings_get_pid(pbio_control_settings_t *s, int32_t *pid_kp, int32_t *pid_ki, int32_t *pid_kd, int32_t *integral_range, int32_t *integral_rate);
 pbio_error_t pbio_control_settings_set_pid(pbio_control_settings_t *s, int32_t pid_kp, int32_t pid_ki, int32_t pid_kd, int32_t integral_range, int32_t integral_rate);

lib/pbio/platform/motors/settings.c

@@ -34,7 +34,8 @@ static const pbio_control_settings_t settings_servo_ev3_m = {
     .pid_kd = 30,
     .integral_range = 45,
     .integral_rate = 10,
-    .max_control = 150000,
+    .max_duty = 10000,
+    .max_torque = 150000,
     .use_estimated_rate = false,
     .use_estimated_count = false,
 };
@@ -63,7 +64,8 @@ static const pbio_control_settings_t settings_servo_ev3_l = {
     .pid_kd = 250,
     .integral_range = 45,
     .integral_rate = 10,
-    .max_control = 430000,
+    .max_duty = 10000,
+    .max_torque = 430000,
     .use_estimated_rate = false,
     .use_estimated_count = false,
 };
@@ -96,7 +98,8 @@ static const pbio_control_settings_t settings_servo_technic_m_angular = {
     .pid_kd = 1200,
     .integral_range = 45,
     .integral_rate = 5,
-    .max_control = 160000,
+    .max_duty = 10000,
+    .max_torque = 160000,
     .use_estimated_rate = true,
     .use_estimated_count = false,
 };
@@ -125,7 +128,8 @@ static const pbio_control_settings_t settings_servo_technic_l_angular = {
     .pid_kd = 4500,
     .integral_range = 45,
     .integral_rate = 5,
-    .max_control = 330000,
+    .max_duty = 10000,
+    .max_torque = 330000,
     .use_estimated_rate = true,
     .use_estimated_count = false,
 };
@@ -154,7 +158,8 @@ static const pbio_control_settings_t settings_servo_interactive = {
     .pid_kd = 1000,
     .integral_range = 45,
     .integral_rate = 3,
-    .max_control = 100000,
+    .max_duty = 10000,
+    .max_torque = 100000,
     .use_estimated_rate = true,
     .use_estimated_count = false,
 };
@@ -185,7 +190,8 @@ static const pbio_control_settings_t settings_servo_movehub = {
     .pid_kd = 500,
     .integral_range = 45,
     .integral_rate = 5,
-    .max_control = 150000,
+    .max_duty = 10000,
+    .max_torque = 150000,
     .use_estimated_rate = true,
     .use_estimated_count = false,
 };
@@ -216,7 +222,8 @@ static const pbio_control_settings_t settings_servo_technic_l = {
     .pid_kd = 1000,
     .integral_range = 45,
     .integral_rate = 5,
-    .max_control = 260000,
+    .max_duty = 10000,
+    .max_torque = 260000,
     .use_estimated_rate = true,
     .use_estimated_count = false,
 };
@@ -245,7 +252,8 @@ static const pbio_control_settings_t settings_servo_technic_xl = {
     .pid_kd = 2000,
     .integral_range = 45,
     .integral_rate = 5,
-    .max_control = 260000,
+    .max_duty = 10000,
+    .max_torque = 260000,
     .use_estimated_rate = true,
     .use_estimated_count = false,
 };

lib/pbio/src/control.c

@@ -15,7 +15,7 @@ void pbio_control_update(pbio_control_t *ctl, int32_t time_now, int32_t count_no
     int32_t time_ref;
     int32_t count_ref, count_ref_ext, count_err, count_feedback, count_err_integral, rate_err_integral;
     int32_t rate_err, rate_feedback;
-    int32_t duty, duty_due_to_proportional, duty_due_to_integral, duty_due_to_derivative;
+    int32_t torque, torque_due_to_proportional, torque_due_to_integral, torque_due_to_derivative;
 
     // Get the time at which we want to evaluate the reference position/velocities.
     // This compensates for any time we may have spent pausing when the motor was stalled.
@@ -43,28 +43,28 @@ void pbio_control_update(pbio_control_t *ctl, int32_t time_now, int32_t count_no
     }
 
     // Corresponding PID control signal
-    duty_due_to_proportional = ctl->settings.pid_kp * count_err;
-    duty_due_to_derivative = ctl->settings.pid_kd * rate_err;
-    duty_due_to_integral = (ctl->settings.pid_ki * (count_err_integral / US_PER_MS)) / MS_PER_SECOND;
+    torque_due_to_proportional = ctl->settings.pid_kp * count_err;
+    torque_due_to_derivative = ctl->settings.pid_kd * rate_err;
+    torque_due_to_integral = (ctl->settings.pid_ki * (count_err_integral / US_PER_MS)) / MS_PER_SECOND;
 
-    // Total duty signal, capped by the actuation limit
-    duty = duty_due_to_proportional + duty_due_to_integral + duty_due_to_derivative;
-    duty = max(-ctl->settings.max_control, min(duty, ctl->settings.max_control));
+    // Total torque signal, capped by the actuation limit
+    torque = torque_due_to_proportional + torque_due_to_integral + torque_due_to_derivative;
+    torque = max(-ctl->settings.max_torque, min(torque, ctl->settings.max_torque));
 
     // This completes the computation of the control signal.
     // The next steps take care of handling windup, or triggering a stop if we are on target.
 
-    // We want to stop building up further errors if we are at the proportional duty limit. So, we pause the trajectory
+    // We want to stop building up further errors if we are at the proportional torque limit. So, we pause the trajectory
     // if we get at this limit. We wait a little longer though, to make sure it does not fall back to below the limit
     // within one sample, which we can predict using the current rate times the loop time, with a factor two tolerance.
-    int32_t max_windup_duty = ctl->settings.max_control + (ctl->settings.pid_kp * abs(rate_now) * PBIO_CONTROL_LOOP_TIME_MS * 2) / MS_PER_SECOND;
+    int32_t max_windup_torque = ctl->settings.max_torque + (ctl->settings.pid_kp * abs(rate_now) * PBIO_CONTROL_LOOP_TIME_MS * 2) / MS_PER_SECOND;
 
-    // Position anti-windup: pause trajectory or integration if falling behind despite using maximum duty
+    // Position anti-windup: pause trajectory or integration if falling behind despite using maximum torque
 
     // Position anti-windup in case of angle control (accumulated position error may not get too high)
     if (ctl->type == PBIO_CONTROL_ANGLE) {
-        if (abs(duty_due_to_proportional) >= max_windup_duty && pbio_math_sign(duty_due_to_proportional) == pbio_math_sign(rate_err)) {
-            // We are at the duty limit and we should prevent further position error integration.
+        if (abs(torque_due_to_proportional) >= max_windup_torque && pbio_math_sign(torque_due_to_proportional) == pbio_math_sign(rate_err)) {
+            // We are at the torque limit and we should prevent further position error integration.
             pbio_count_integrator_pause(&ctl->count_integrator, time_now, count_now, count_ref);
         } else {
             // Not at the limit so continue integrating errors
@@ -73,8 +73,8 @@ void pbio_control_update(pbio_control_t *ctl, int32_t time_now, int32_t count_no
     }
     // Position anti-windup in case of timed speed control (speed integral may not get too high)
     else {
-        if (abs(duty_due_to_proportional) >= max_windup_duty && pbio_math_sign(duty_due_to_proportional) == pbio_math_sign(rate_err)) {
-            // We are at the duty limit and we should prevent further speed error integration.
+        if (abs(torque_due_to_proportional) >= max_windup_torque && pbio_math_sign(torque_due_to_proportional) == pbio_math_sign(rate_err)) {
+            // We are at the torque limit and we should prevent further speed error integration.
             pbio_rate_integrator_pause(&ctl->rate_integrator, time_now, count_now, count_ref);
         } else {
             // Not at the limit so continue integrating errors
@@ -102,11 +102,11 @@ void pbio_control_update(pbio_control_t *ctl, int32_t time_now, int32_t count_no
 
         // The new hold control does not take effect until the next iteration, so keep actuating for now.
         *actuation = PBIO_ACTUATION_DUTY;
-        *control = duty;
+        *control = torque;
     } else {
-        // The end point not reached, or we have to keep holding, so return the calculated duty for actuation
+        // The end point not reached, or we have to keep holding, so return the calculated torque for actuation
         *actuation = PBIO_ACTUATION_DUTY;
-        *control = duty;
+        *control = torque;
     }
 
     // Log control data
@@ -120,9 +120,9 @@ void pbio_control_update(pbio_control_t *ctl, int32_t time_now, int32_t count_no
         *rate_ref,
         count_est,
         rate_est,
-        duty_due_to_proportional,
-        duty_due_to_integral,
-        duty_due_to_derivative,
+        torque_due_to_proportional,
+        torque_due_to_integral,
+        torque_due_to_derivative,
     };
     pbio_logger_update(&ctl->log, log_data);
 }
@@ -326,17 +326,21 @@ int32_t pbio_control_user_to_counts(pbio_control_settings_t *s, int32_t user) {
     return pbio_math_mul_i32_fix16(user, s->counts_per_unit);
 }
 
-void pbio_control_settings_get_limits(pbio_control_settings_t *s, int32_t *speed, int32_t *acceleration, int32_t *actuation) {
+void pbio_control_settings_get_limits(pbio_control_settings_t *s, int32_t *speed, int32_t *acceleration, int32_t *duty, int32_t *torque) {
     *speed = pbio_control_counts_to_user(s, s->max_rate);
     *acceleration = pbio_control_counts_to_user(s, s->abs_acceleration);
+    *duty = s->max_duty / 100;
+    *torque = s->max_torque;
 }
 
-pbio_error_t pbio_control_settings_set_limits(pbio_control_settings_t *s, int32_t speed, int32_t acceleration, int32_t actuation) {
-    if (speed < 1 || acceleration < 1 || actuation < 1) {
+pbio_error_t pbio_control_settings_set_limits(pbio_control_settings_t *s, int32_t speed, int32_t acceleration, int32_t duty, int32_t torque) {
+    if (speed < 1 || acceleration < 1 || duty < 1 || torque < 1 || duty > 100) {
         return PBIO_ERROR_INVALID_ARG;
     }
     s->max_rate = pbio_control_user_to_counts(s, speed);
     s->abs_acceleration = pbio_control_user_to_counts(s, acceleration);
+    s->max_duty = duty * 100;
+    s->max_torque = torque;
     return PBIO_SUCCESS;
 }
 
@@ -396,8 +400,8 @@ int32_t pbio_control_settings_get_max_integrator(pbio_control_settings_t *s) {
     if (s->pid_ki <= 10) {
         return 1000000000;
     }
-    // Get the maximum integrator value for which ki*integrator does not exceed max_control
-    return ((s->max_control * US_PER_MS) / s->pid_ki) * MS_PER_SECOND;
+    // Get the maximum integrator value for which ki*integrator does not exceed max_torque
+    return ((s->max_torque * US_PER_MS) / s->pid_ki) * MS_PER_SECOND;
 }
 
 int32_t pbio_control_get_ref_time(pbio_control_t *ctl, int32_t time_now) {

lib/pbio/src/drivebase.c

@@ -32,7 +32,7 @@ static pbio_error_t drivebase_adopt_settings(pbio_control_settings_t *s_distance
     s_distance->pid_kd = (s_left->pid_kd + s_right->pid_kd) / 2;
 
     // Maxima are bound by the least capable motor
-    s_distance->max_control = min(s_left->max_control, s_right->max_control);
+    s_distance->max_torque = min(s_left->max_torque, s_right->max_torque);
     s_distance->stall_time = min(s_left->stall_time, s_right->stall_time);
 
     // Copy rate estimator usage, required to be the same on both motors

pybricks/common/pb_type_control.c

@@ -56,19 +56,21 @@ STATIC mp_obj_t common_Control_limits(size_t n_args, const mp_obj_t *pos_args, m
         common_Control_obj_t, self,
         PB_ARG_DEFAULT_NONE(speed),
         PB_ARG_DEFAULT_NONE(acceleration),
-        PB_ARG_DEFAULT_NONE(actuation));
+        PB_ARG_DEFAULT_NONE(duty),
+        PB_ARG_DEFAULT_NONE(torque));
 
     // Read current values
-    int32_t speed, acceleration, actuation;
-    pbio_control_settings_get_limits(&self->control->settings, &speed, &acceleration, &actuation);
+    int32_t speed, acceleration, duty, torque;
+    pbio_control_settings_get_limits(&self->control->settings, &speed, &acceleration, &duty, &torque);
 
     // If all given values are none, return current values
-    if (speed_in == mp_const_none && acceleration_in == mp_const_none && actuation_in == mp_const_none) {
-        mp_obj_t ret[3];
+    if (speed_in == mp_const_none && acceleration_in == mp_const_none && duty_in == mp_const_none && torque_in == mp_const_none) {
+        mp_obj_t ret[4];
         ret[0] = mp_obj_new_int(speed);
         ret[1] = mp_obj_new_int(acceleration);
-        ret[2] = mp_obj_new_int(actuation);
-        return mp_obj_new_tuple(3, ret);
+        ret[2] = mp_obj_new_int(duty);
+        ret[3] = mp_obj_new_int(torque);
+        return mp_obj_new_tuple(4, ret);
     }
 
     // Assert control is not active
@@ -77,9 +79,10 @@ STATIC mp_obj_t common_Control_limits(size_t n_args, const mp_obj_t *pos_args, m
     // Set user settings
     speed = pb_obj_get_default_int(speed_in, speed);
     acceleration = pb_obj_get_default_int(acceleration_in, acceleration);
-    actuation = pb_obj_get_default_int(actuation_in, actuation);
+    duty = pb_obj_get_default_int(duty_in, duty);
+    torque = pb_obj_get_default_int(torque_in, torque);
 
-    pb_assert(pbio_control_settings_set_limits(&self->control->settings, speed, acceleration, actuation));
+    pb_assert(pbio_control_settings_set_limits(&self->control->settings, speed, acceleration, duty, torque));
 
     return mp_const_none;
 }

pybricks/common/pb_type_motor.c

@@ -206,22 +206,22 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po
     mp_int_t speed = pb_obj_get_int(speed_in);
     pbio_actuation_t then = pb_type_enum_get_value(then_in, &pb_enum_type_Stop);
 
-    // If duty_limit argument, given, limit actuation during this maneuver
+    // If duty_limit argument, given, limit duty during this maneuver
     bool override_duty_limit = duty_limit_in != mp_const_none;
 
-    int32_t orig_speed, acceleration, actuation;
+    int32_t orig_speed, acceleration, duty, torque;
 
     if (override_duty_limit) {
         // Read original values so we can restore them when we're done
-        pbio_control_settings_get_limits(&self->srv->control.settings, &orig_speed, &acceleration, &actuation);
+        pbio_control_settings_get_limits(&self->srv->control.settings, &orig_speed, &acceleration, &duty, &torque);
 
         // Get user given limit
         mp_int_t duty_limit = pb_obj_get_int(duty_limit_in);
         duty_limit = duty_limit < 0 ? -duty_limit : duty_limit;
         duty_limit = duty_limit > 100 ? 100 : duty_limit;
 
         // Apply the user limit
-        pb_assert(pbio_control_settings_set_limits(&self->srv->control.settings, orig_speed, acceleration, duty_limit));
+        pb_assert(pbio_control_settings_set_limits(&self->srv->control.settings, orig_speed, acceleration, duty_limit, torque));
     }
 
     mp_obj_t ex = MP_OBJ_NULL;
@@ -241,7 +241,7 @@ STATIC mp_obj_t common_Motor_run_until_stalled(size_t n_args, const mp_obj_t *po
 
     // Restore original settings
     if (override_duty_limit) {
-        pb_assert(pbio_control_settings_set_limits(&self->srv->control.settings, orig_speed, acceleration, actuation));
+        pb_assert(pbio_control_settings_set_limits(&self->srv->control.settings, orig_speed, acceleration, duty, torque));
     }
 
     if (ex != MP_OBJ_NULL) {

pybricks/robotics/pb_type_drivebase.c

@@ -68,8 +68,8 @@ STATIC mp_obj_t robotics_DriveBase_make_new(const mp_obj_type_t *type, size_t n_
 
     // Get defaults for drivebase as 1/3 of maximum for the underlying motors
     int32_t straight_speed_limit, straight_acceleration_limit, turn_rate_limit, turn_acceleration_limit, _;
-    pbio_control_settings_get_limits(&self->db->control_distance.settings, &straight_speed_limit, &straight_acceleration_limit, &_);
-    pbio_control_settings_get_limits(&self->db->control_heading.settings, &turn_rate_limit, &turn_acceleration_limit, &_);
+    pbio_control_settings_get_limits(&self->db->control_distance.settings, &straight_speed_limit, &straight_acceleration_limit, &_, &_);
+    pbio_control_settings_get_limits(&self->db->control_heading.settings, &turn_rate_limit, &turn_acceleration_limit, &_, &_);
 
     self->straight_speed = straight_speed_limit / 3;
     self->straight_acceleration = straight_acceleration_limit / 3;
@@ -220,8 +220,8 @@ STATIC mp_obj_t robotics_DriveBase_settings(size_t n_args, const mp_obj_t *pos_a
 
     // If some values are given, set them, bound by the control limits
     int32_t straight_speed_limit, straight_acceleration_limit, turn_rate_limit, turn_acceleration_limit, _;
-    pbio_control_settings_get_limits(&self->db->control_distance.settings, &straight_speed_limit, &straight_acceleration_limit, &_);
-    pbio_control_settings_get_limits(&self->db->control_heading.settings, &turn_rate_limit, &turn_acceleration_limit, &_);
+    pbio_control_settings_get_limits(&self->db->control_distance.settings, &straight_speed_limit, &straight_acceleration_limit, &_, &_);
+    pbio_control_settings_get_limits(&self->db->control_heading.settings, &turn_rate_limit, &turn_acceleration_limit, &_, &_);
 
     self->straight_speed = min(straight_speed_limit, abs(pb_obj_get_default_int(straight_speed_in, self->straight_speed)));
     self->straight_acceleration = min(straight_acceleration_limit, abs(pb_obj_get_default_int(straight_acceleration_in, self->straight_acceleration)));