91 lines
2.3 KiB
C
91 lines
2.3 KiB
C
|
/* Reflow Oven Controller
|
||
|
|
*
|
||
|
|
* Copyright (C) 2020 Mario Hüttel <mario.huettel@gmx.net>
|
||
|
|
*
|
||
|
|
* 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 <http://www.gnu.org/licenses/>.
|
||
|
|
*/
|
||
|
|
|
||
|
|
#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)
|
||
|
|
{
|
||
|
|
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;
|
||
|
|
}
|