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

make PID controller mathemtaically correct

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_int;
float k_p;
float k_int_t;
float k_deriv_t;
float output_sat_max;
float output_sat_min;
float integral_max;
float sample_period;
volatile float control_output;
volatile float last_in;
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);

2
stm-firmware/main.c
View File

@ -221,7 +221,7 @@ int main()
shell_handle = shell_init(write_shell_callback);
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);
while (1) {

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

@ -20,18 +20,22 @@
#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)
return;
pid->sample_period = sample_period;
pid->k_p = k_p;
pid->k_int = k_int;
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_min = output_sat_min;
pid->control_output = 0;
pid->integral_max = integral_max;
pid_zero(pid);
}
void pid_zero(struct pid_controller *pid)
@ -43,12 +47,13 @@ void pid_zero(struct pid_controller *pid)
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) {
pid->integral = pid->integral_max;
} else if (pid->integral < -pid->integral_max) {
pid->integral = -pid->integral_max;
} else if (pid->integral < -pid->integral_max){
pid->integral = - pid->integral_max;
}
}
@ -66,17 +71,20 @@ float pid_sample(struct pid_controller *pid, float 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 -= (deviation - pid->last_in) * pid->k_deriv;
pid->last_in = deviation;
if (output > pid->output_sat_max)
output = pid->output_sat_max;
/* Saturate output */
if (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->last_in = deviation;
return output;
}