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; }