pbio/control: Drop integral range setting.

This was used to define when integral control is active. With this commit, this range is computed automatically. This ensures that the integrator becomes active in time if proportional control is reduced.

lib/pbio/include/pbio/control.h

@@ -37,7 +37,6 @@ typedef struct _pbio_control_settings_t {
     int32_t pid_kd;                 /**< Derivative position control constant (and proportional speed control constant) */
     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 */
     bool use_estimated_count;       /**< Whether to use the estimated count (true) or the reported/measured count (false) for feedback control */
@@ -92,8 +91,8 @@ 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 *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);
+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_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_rate);
 
 void pbio_control_settings_get_target_tolerances(pbio_control_settings_t *s, int32_t *speed, int32_t *position);
 pbio_error_t pbio_control_settings_set_target_tolerances(pbio_control_settings_t *s, int32_t speed, int32_t position);

lib/pbio/platform/motors/settings.c

@@ -38,7 +38,6 @@ static const pbio_control_settings_t settings_servo_ev3_m = {
     .pid_kp = 3000,
     .pid_ki = 150,
     .pid_kd = 30,
-    .integral_range = 45,
     .integral_rate = 10,
     .max_duty = 10000,
     .max_torque = 150000,
@@ -68,7 +67,6 @@ static const pbio_control_settings_t settings_servo_ev3_l = {
     .pid_kp = 15000,
     .pid_ki = 600,
     .pid_kd = 250,
-    .integral_range = 45,
     .integral_rate = 10,
     .max_duty = 10000,
     .max_torque = 430000,
@@ -102,7 +100,6 @@ static const pbio_control_settings_t settings_servo_technic_s_angular = {
     .pid_kp = 5000,
     .pid_ki = 1200,
     .pid_kd = 800,
-    .integral_range = 45,
     .integral_rate = 25,
     .max_duty = 7500,
     .max_torque = 60000,
@@ -132,7 +129,6 @@ static const pbio_control_settings_t settings_servo_technic_m_angular = {
     .pid_kp = 10000,
     .pid_ki = 2000,
     .pid_kd = 1200,
-    .integral_range = 45,
     .integral_rate = 25,
     .max_duty = 10000,
     .max_torque = 160000,
@@ -162,7 +158,6 @@ static const pbio_control_settings_t settings_servo_technic_l_angular = {
     .pid_kp = 25000,
     .pid_ki = 6000,
     .pid_kd = 4500,
-    .integral_range = 45,
     .integral_rate = 5,
     .max_duty = 10000,
     .max_torque = 330000,
@@ -192,7 +187,6 @@ static const pbio_control_settings_t settings_servo_interactive = {
     .pid_kp = 10000,
     .pid_ki = 1000,
     .pid_kd = 1000,
-    .integral_range = 45,
     .integral_rate = 3,
     .max_duty = 10000,
     .max_torque = 100000,
@@ -224,7 +218,6 @@ static const pbio_control_settings_t settings_servo_movehub = {
     .pid_kp = 15000,
     .pid_ki = 1500,
     .pid_kd = 500,
-    .integral_range = 45,
     .integral_rate = 5,
     .max_duty = 10000,
     .max_torque = 150000,
@@ -256,7 +249,6 @@ static const pbio_control_settings_t settings_servo_technic_l = {
     .pid_kp = 4000,
     .pid_ki = 600,
     .pid_kd = 1000,
-    .integral_range = 45,
     .integral_rate = 5,
     .max_duty = 10000,
     .max_torque = 260000,
@@ -286,7 +278,6 @@ static const pbio_control_settings_t settings_servo_technic_xl = {
     .pid_kp = 2500,
     .pid_ki = 400,
     .pid_kd = 2000,
-    .integral_range = 45,
     .integral_rate = 5,
     .max_duty = 10000,
     .max_torque = 260000,

lib/pbio/src/control.c

@@ -30,8 +30,14 @@ void pbio_control_update(pbio_control_t *ctl, int32_t time_now, int32_t count_no
 
     // Calculate control errors, depending on whether we do angle control or speed control
     if (ctl->type == PBIO_CONTROL_ANGLE) {
+
+        // Specify in which region integral control should be active. This is
+        // at least the error that would still lead to maximum  proportional
+        // control, with a factor of 2 so we begin integrating a bit sooner.
+        int32_t integral_range = (ctl->settings.max_torque / ctl->settings.pid_kp) * 2;
+
         // Update count integral error and get current error state
-        pbio_count_integrator_update(&ctl->count_integrator, time_now, count_now, count_ref, ctl->trajectory.th3, ctl->settings.integral_range, ctl->settings.integral_rate);
+        pbio_count_integrator_update(&ctl->count_integrator, time_now, count_now, count_ref, ctl->trajectory.th3, integral_range, ctl->settings.integral_rate);
         pbio_count_integrator_get_errors(&ctl->count_integrator, count_feedback, count_ref, &count_err, &count_err_integral);
         rate_err = *rate_ref - rate_feedback;
     } else {
@@ -356,23 +362,21 @@ pbio_error_t pbio_control_settings_set_limits(pbio_control_settings_t *s, int32_
     return PBIO_SUCCESS;
 }
 
-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) {
+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_rate) {
     *pid_kp = s->pid_kp;
     *pid_ki = s->pid_ki;
     *pid_kd = s->pid_kd;
-    *integral_range = pbio_control_counts_to_user(s, s->integral_range);
     *integral_rate = pbio_control_counts_to_user(s, s->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) {
-    if (pid_kp < 0 || pid_ki < 0 || pid_kd < 0 || integral_range < 0 || integral_rate < 0) {
+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_rate) {
+    if (pid_kp < 0 || pid_ki < 0 || pid_kd < 0 || integral_rate < 0) {
         return PBIO_ERROR_INVALID_ARG;
     }
 
     s->pid_kp = pid_kp;
     s->pid_ki = pid_ki;
     s->pid_kd = pid_kd;
-    s->integral_range = pbio_control_user_to_counts(s, integral_range);
     s->integral_rate = pbio_control_user_to_counts(s, integral_rate);
     return PBIO_SUCCESS;
 }

lib/pbio/src/drivebase.c

@@ -19,7 +19,6 @@ static pbio_error_t drivebase_adopt_settings(pbio_control_settings_t *s_distance
     s_distance->rate_tolerance = s_left->rate_tolerance + s_right->rate_tolerance;
     s_distance->count_tolerance = s_left->count_tolerance + s_right->count_tolerance;
     s_distance->stall_rate_limit = s_left->stall_rate_limit + s_right->stall_rate_limit;
-    s_distance->integral_range = s_left->integral_range + s_right->integral_range;
     s_distance->integral_rate = s_left->integral_rate + s_right->integral_rate;
 
     // As acceleration, we take double the single motor amount, because drivebases are

pybricks/common/pb_type_control.c

@@ -96,24 +96,22 @@ STATIC mp_obj_t common_Control_pid(size_t n_args, const mp_obj_t *pos_args, mp_m
         PB_ARG_DEFAULT_NONE(kp),
         PB_ARG_DEFAULT_NONE(ki),
         PB_ARG_DEFAULT_NONE(kd),
-        PB_ARG_DEFAULT_NONE(integral_range),
         PB_ARG_DEFAULT_NONE(integral_rate));
 
     // Read current values
     int32_t kp, ki, kd;
-    int32_t integral_range, integral_rate;
-    pbio_control_settings_get_pid(&self->control->settings, &kp, &ki, &kd, &integral_range, &integral_rate);
+    int32_t integral_rate;
+    pbio_control_settings_get_pid(&self->control->settings, &kp, &ki, &kd, &integral_rate);
 
     // If all given values are none, return current values
     if (kp_in == mp_const_none && ki_in == mp_const_none && kd_in == mp_const_none &&
-        integral_range_in == mp_const_none && integral_rate_in == mp_const_none) {
-        mp_obj_t ret[5];
+        integral_rate_in == mp_const_none) {
+        mp_obj_t ret[4];
         ret[0] = mp_obj_new_int(kp);
         ret[1] = mp_obj_new_int(ki);
         ret[2] = mp_obj_new_int(kd);
-        ret[3] = mp_obj_new_int(integral_range);
-        ret[4] = mp_obj_new_int(integral_rate);
-        return mp_obj_new_tuple(5, ret);
+        ret[3] = mp_obj_new_int(integral_rate);
+        return mp_obj_new_tuple(4, ret);
     }
 
     // Assert control is not active
@@ -123,10 +121,9 @@ STATIC mp_obj_t common_Control_pid(size_t n_args, const mp_obj_t *pos_args, mp_m
     kp = pb_obj_get_default_int(kp_in, kp);
     ki = pb_obj_get_default_int(ki_in, ki);
     kd = pb_obj_get_default_int(kd_in, kd);
-    integral_range = pb_obj_get_default_int(integral_range_in, integral_range);
     integral_rate = pb_obj_get_default_int(integral_rate_in, integral_rate);
 
-    pb_assert(pbio_control_settings_set_pid(&self->control->settings, kp, ki, kd, integral_range, integral_rate));
+    pb_assert(pbio_control_settings_set_pid(&self->control->settings, kp, ki, kd, integral_rate));
 
     return mp_const_none;
 }