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

make PID controller mathemtaically correct

Browse Source
This commit is contained in:
Mario Hüttel 2020-05-25 01:56:54 +02:00
parent 355e81ba44
commit e659c6d097
3 changed files with 24 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
stm-firmware/include/reflow-controller/pid-controller.h
View File

@ -25,15 +25,19 @@ struct pid_controller {
float k_deriv; float k_deriv;
float k_int; float k_int;
float k_p; float k_p;
float k_int_t;
float k_deriv_t;
float output_sat_max; float output_sat_max;
float output_sat_min; float output_sat_min;
float integral_max; float integral_max;
float sample_period;
volatile float control_output; volatile float control_output;
volatile float last_in; volatile float last_in;
volatile float integral; volatile float integral;
volatile float derivate;
}; };
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); 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);
void pid_zero(struct pid_controller *pid); void pid_zero(struct pid_controller *pid);

2
stm-firmware/main.c
View File

@ -221,7 +221,7 @@ int main()
shell_handle = shell_init(write_shell_callback); shell_handle = shell_init(write_shell_callback);
shell_print_motd(shell_handle); shell_print_motd(shell_handle);
pid_init(&pid, 0.1, 0.1, 4.0, 0.0, 100.0, 40.0); pid_init(&pid, 0.1, 0.1, 4.0, 0.0, 100.0, 40.0, 0.25);
pid_zero(&pid); pid_zero(&pid);
while (1) { while (1) {

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

@ -20,18 +20,22 @@
#include <reflow-controller/pid-controller.h> #include <reflow-controller/pid-controller.h>
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) 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)
{ {
if (!pid) if (!pid)
return; return;
pid->sample_period = sample_period;
pid->k_p = k_p; pid->k_p = k_p;
pid->k_int = k_int; pid->k_int = k_int;
pid->k_deriv = k_deriv; pid->k_deriv = k_deriv;
pid->k_int_t = pid->k_int * pid->sample_period / 2.0f;
pid->k_deriv_t = pid->k_deriv * 2.0f / pid->sample_period;
pid->output_sat_max = output_sat_max; pid->output_sat_max = output_sat_max;
pid->output_sat_min = output_sat_min; pid->output_sat_min = output_sat_min;
pid->control_output = 0;
pid->integral_max = integral_max; pid->integral_max = integral_max;
pid_zero(pid);
} }
void pid_zero(struct pid_controller *pid) void pid_zero(struct pid_controller *pid)
@ -43,8 +47,9 @@ void pid_zero(struct pid_controller *pid)
static void calculate_integral(struct pid_controller *pid, float deviation) static void calculate_integral(struct pid_controller *pid, float deviation)
{ {
pid->integral += deviation * pid->k_int; pid->integral = pid->integral + pid->k_int_t * (deviation + pid->last_in);
/* Saturate integral term to spoecified maximum */
if (pid->integral > pid->integral_max) { if (pid->integral > pid->integral_max) {
pid->integral = pid->integral_max; pid->integral = pid->integral_max;
} else if (pid->integral < -pid->integral_max){ } else if (pid->integral < -pid->integral_max){
@ -66,17 +71,20 @@ float pid_sample(struct pid_controller *pid, float deviation)
calculate_integral(pid, deviation); calculate_integral(pid, deviation);
} }
/* Calculate derivative part */
pid->derivate = pid->k_deriv_t * (deviation - pid->last_in) - pid->derivate;
output += pid->derivate;
output += pid->integral; output += pid->integral;
output -= (deviation - pid->last_in) * pid->k_deriv;
pid->last_in = deviation; /* Saturate output */
if (output > pid->output_sat_max)
output = pid->output_sat_max;
if (output < pid->output_sat_min) if (output < pid->output_sat_min)
output = pid->output_sat_min; output = pid->output_sat_min;
else if (output > pid->output_sat_max)
output = pid->output_sat_max;
pid->control_output = output; pid->control_output = output;
pid->last_in = deviation;
return output; return output;
} }