mhu
/
reflow-oven-control-sw
2
0
Fork 0
Code Issues 4 Pull Requests Releases 4 Wiki Activity

Add low pass for Derviative part in PID controller

This commit is contained in:
Mario Hüttel 2020-11-30 21:40:28 +01:00
parent 91d9db6a4e
commit cd35f9e694
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
stm-firmware/pid-controller.c
View File

@ -21,6 +21,8 @@
#include <reflow-controller/pid-controller.h>
#include <string.h>
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;