From cd35f9e694869970a45d91e7002a77fa34daae91 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mario=20H=C3=BCttel?= Date: Mon, 30 Nov 2020 21:40:28 +0100 Subject: [PATCH] Add low pass for Derviative part in PID controller --- stm-firmware/pid-controller.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/stm-firmware/pid-controller.c b/stm-firmware/pid-controller.c index 463008f..f66efa8 100644 --- a/stm-firmware/pid-controller.c +++ b/stm-firmware/pid-controller.c @@ -21,6 +21,8 @@ #include #include +const float kd_tau = 2.0f; + void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, float output_sat_min, float output_sat_max, float integral_max, float sample_period) { @@ -32,7 +34,8 @@ void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, pid->k_int = k_int; pid->k_deriv = k_deriv; pid->k_int_t = pid->k_int * pid->sample_period * 0.5f; - pid->k_deriv_t = pid->k_deriv * 2.0f / pid->sample_period; + pid->k_deriv_t = pid->k_deriv * 2.0f / (pid->sample_period + 2 * kd_tau); + pid->k_inv_deriv_t = (2 * kd_tau - sample_period) / (2 * kd_tau + sample_period); pid->output_sat_max = output_sat_max; pid->output_sat_min = output_sat_min; pid->integral_max = integral_max; @@ -61,7 +64,7 @@ static void calculate_integral(struct pid_controller *pid, float deviation) { pid->integral = pid->integral + pid->k_int_t * (deviation + pid->last_in); - /* Saturate integral term to spoecified maximum */ + /* Saturate integral term to specified maximum */ if (pid->integral > pid->integral_max) pid->integral = pid->integral_max; else if (pid->integral < -pid->integral_max) @@ -78,13 +81,13 @@ float pid_sample(struct pid_controller *pid, float deviation) output = deviation * pid->k_p; /* PID runaway compensation */ - if (!(deviation > 0.0f && pid->control_output > pid->output_sat_max - 0.5f) && - !(deviation < 0.0f && pid->control_output < pid->output_sat_min + 0.5f)) { + if (!(deviation > 0.0f && pid->control_output > pid->output_sat_max - 0.5f && pid->integral > 0) && + !(deviation < 0.0f && pid->control_output < pid->output_sat_min + 0.5f && pid->integral < 0)) { calculate_integral(pid, deviation); } /* Calculate derivative part */ - pid->derivate = pid->k_deriv_t * (deviation - pid->last_in) - pid->derivate; + pid->derivate = pid->k_deriv_t * (deviation - pid->last_in) - pid->k_inv_deriv_t * pid->derivate; output += pid->derivate; output += pid->integral;