From 3f0042f988127a2d88031f920b7f9c943c301f61 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 21 Sep 2021 11:56:47 +0200 Subject: [PATCH] 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 | 5 ++--- lib/pbio/platform/motors/settings.c | 9 --------- lib/pbio/src/control.c | 16 ++++++++++------ lib/pbio/src/drivebase.c | 1 - pybricks/common/pb_type_control.c | 17 +++++++---------- 5 files changed, 19 insertions(+), 29 deletions(-) diff --git a/lib/pbio/include/pbio/control.h b/lib/pbio/include/pbio/control.h index 7aebe07fd..5cc603aef 100644 --- a/lib/pbio/include/pbio/control.h +++ b/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); diff --git a/lib/pbio/platform/motors/settings.c b/lib/pbio/platform/motors/settings.c index 4fee32fff..57d37f5c5 100644 --- a/lib/pbio/platform/motors/settings.c +++ b/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, diff --git a/lib/pbio/src/control.c b/lib/pbio/src/control.c index 7a951a674..b1055f62b 100644 --- a/lib/pbio/src/control.c +++ b/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; } diff --git a/lib/pbio/src/drivebase.c b/lib/pbio/src/drivebase.c index a58d0f2ec..f8ca0e846 100644 --- a/lib/pbio/src/drivebase.c +++ b/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 diff --git a/pybricks/common/pb_type_control.c b/pybricks/common/pb_type_control.c index f20540711..83d52401e 100644 --- a/pybricks/common/pb_type_control.c +++ b/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; }