pbio/control: subtract offset from windup duty
The maximum proportional count error at which it exerts full duty is (max-offset)/kp, so set it accordingly.
@@ -55,10 +55,10 @@ void control_update(pbio_control_t *ctl, int32_t time_now, int32_t count_now, in
// We want to stop building up further errors if we are at the proportional duty 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_duty = ctl->settings.max_control + (ctl->settings.pid_kp * abs(rate_now) * PBIO_CONFIG_SERVO_PERIOD_MS * 2) / MS_PER_SECOND;
+ int32_t max_windup_duty = (ctl->settings.max_control - ctl->settings.control_offset) + (ctl->settings.pid_kp * abs(rate_now) * PBIO_CONFIG_SERVO_PERIOD_MS * 2) / MS_PER_SECOND;
// Position anti-windup: pause trajectory or integration if falling behind despite using maximum duty
- if (abs(duty_due_to_proportional) >= max_duty) {
+ if (abs(duty_due_to_proportional) >= max_windup_duty) {
// We are at the duty limit and we should prevent further position error "integration".
if (ctl->type == PBIO_CONTROL_ANGLE) {
pbio_count_integrator_pause(&ctl->count_integrator, time_now, count_now, count_ref);
