From c419ca7bcbeeb229f518a402d4cf3eeb54deb8ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mario=20H=C3=BCttel?= Date: Mon, 20 Apr 2020 21:16:39 +0200 Subject: [PATCH] Add PID controller and oven driver module --- stm-firmware/Makefile | 2 + .../include/reflow-controller/oven-driver.h | 24 +++++ .../reflow-controller/pid-controller.h | 44 +++++++++ stm-firmware/oven-driver.c | 21 +++++ stm-firmware/pid-controller.c | 90 +++++++++++++++++++ 5 files changed, 181 insertions(+) create mode 100644 stm-firmware/include/reflow-controller/oven-driver.h create mode 100644 stm-firmware/include/reflow-controller/pid-controller.h create mode 100644 stm-firmware/oven-driver.c create mode 100644 stm-firmware/pid-controller.c diff --git a/stm-firmware/Makefile b/stm-firmware/Makefile index 44161ac..fc35fde 100644 --- a/stm-firmware/Makefile +++ b/stm-firmware/Makefile @@ -53,6 +53,8 @@ CFILES += stack-check.c CFILES += fatfs/diskio.c fatfs/ff.c fatfs/ffsystem.c fatfs/ffunicode.c fatfs/shimatta_sdio_driver/shimatta_sdio.c +CFILES += pid-controller.c oven-driver.c + DEFINES += -DDEBUGBUILD ################################################################################### diff --git a/stm-firmware/include/reflow-controller/oven-driver.h b/stm-firmware/include/reflow-controller/oven-driver.h new file mode 100644 index 0000000..0ba0610 --- /dev/null +++ b/stm-firmware/include/reflow-controller/oven-driver.h @@ -0,0 +1,24 @@ +/* Reflow Oven Controller +* +* Copyright (C) 2020 Mario Hüttel +* +* This file is part of the Reflow Oven Controller Project. +* +* The reflow oven controller is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License version 2 as +* published by the Free Software Foundation. +* +* The Reflow Oven Control Firmware is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with the reflow oven controller project. +* If not, see . +*/ + +#ifndef __OVEN_DRIVER_H__ +#define __OVEN_DRIVER_H__ + +#endif /* __OVEN_DRIVER_H__ */ diff --git a/stm-firmware/include/reflow-controller/pid-controller.h b/stm-firmware/include/reflow-controller/pid-controller.h new file mode 100644 index 0000000..1e64741 --- /dev/null +++ b/stm-firmware/include/reflow-controller/pid-controller.h @@ -0,0 +1,44 @@ +/* Reflow Oven Controller +* +* Copyright (C) 2020 Mario Hüttel +* +* This file is part of the Reflow Oven Controller Project. +* +* The reflow oven controller is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License version 2 as +* published by the Free Software Foundation. +* +* The Reflow Oven Control Firmware is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with the reflow oven controller project. +* If not, see . +*/ + +#ifndef __PID_CONTROLLER_H__ +#define __PID_CONTROLLER_H__ + +struct pid_controller { + float k_deriv; + float k_int; + float k_p; + float output_sat_max; + float output_sat_min; + float integral_max; + volatile float control_output; + volatile float last_in; + volatile float integral; +}; + +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_zero(struct pid_controller *pid); + +float pid_sample(struct pid_controller *pid, float deviation); + +float pid_get_control_output(const struct pid_controller *pid); + +#endif /* __PID_CONTROLLER_H__ */ diff --git a/stm-firmware/oven-driver.c b/stm-firmware/oven-driver.c new file mode 100644 index 0000000..27a7564 --- /dev/null +++ b/stm-firmware/oven-driver.c @@ -0,0 +1,21 @@ +/* Reflow Oven Controller +* +* Copyright (C) 2020 Mario Hüttel +* +* This file is part of the Reflow Oven Controller Project. +* +* The reflow oven controller is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License version 2 as +* published by the Free Software Foundation. +* +* The Reflow Oven Control Firmware is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with the reflow oven controller project. +* If not, see . +*/ + +#include diff --git a/stm-firmware/pid-controller.c b/stm-firmware/pid-controller.c new file mode 100644 index 0000000..ef97235 --- /dev/null +++ b/stm-firmware/pid-controller.c @@ -0,0 +1,90 @@ +/* Reflow Oven Controller +* +* Copyright (C) 2020 Mario Hüttel +* +* This file is part of the Reflow Oven Controller Project. +* +* The reflow oven controller is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License version 2 as +* published by the Free Software Foundation. +* +* The Reflow Oven Control Firmware is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with the reflow oven controller project. +* If not, see . +*/ + +#include + +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) +{ + if (!pid) + return; + + pid->k_p = k_p; + pid->k_int = k_int; + pid->k_deriv = k_deriv; + pid->output_sat_max = output_sat_max; + pid->output_sat_min = output_sat_min; + pid->control_output = 0; + pid->integral_max = integral_max; +} + +void pid_zero(struct pid_controller *pid) +{ + pid->control_output = 0.0f; + pid->last_in = 0.0f; + pid->integral = 0.0f; +} + +static void calculate_integral(struct pid_controller *pid, float deviation) +{ + pid->integral += deviation * pid->k_int; + + if (pid->integral > pid->integral_max) { + pid->integral = pid->integral_max; + } else if (pid->integral < -pid->integral_max) { + pid->integral = -pid->integral_max; + } +} + +float pid_sample(struct pid_controller *pid, float deviation) +{ + float output; + + if (!pid) + return 0.0; + + output = deviation * pid->k_p; + + if (!(deviation > 0.0f && pid->control_output > pid->output_sat_max - 0.5) && + !(deviation < 0.0f && pid->control_output < pid->output_sat_min + 0.5)) { + calculate_integral(pid, deviation); + } + + 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; + if (output < pid->output_sat_min) + output = pid->output_sat_min; + + pid->control_output = output; + + return output; +} + +float pid_get_control_output(const struct pid_controller *pid) +{ + if (!pid) + return 0.0f; + + return pid->control_output; +}