mirror of
https://github.com/grblHAL/core.git
synced 2026-02-06 00:52:35 +08:00
106 lines
2.8 KiB
C
106 lines
2.8 KiB
C
/*
|
|
pid.c - An embedded CNC Controller with rs274/ngc (g-code) support
|
|
|
|
PID algorithm for closed loop control
|
|
|
|
NOTE: not referenced in the core grbl code
|
|
|
|
Part of grblHAL
|
|
|
|
Copyright (c) 2020-2021 Terje Io
|
|
|
|
Grbl is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
Grbl 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 Grbl. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <string.h>
|
|
|
|
#include "pid.h"
|
|
|
|
// Fixed point version: TODO
|
|
|
|
// Float version
|
|
|
|
void pidf_init (pidf_t *pid, pid_values_t *config)
|
|
{
|
|
pidf_reset(pid);
|
|
memcpy(&pid->cfg, config, sizeof(pid_values_t));
|
|
}
|
|
|
|
bool pidf_config_changed (pidf_t *pid, pid_values_t *config)
|
|
{
|
|
return memcmp(&pid->cfg, config, sizeof(pid_values_t));
|
|
}
|
|
|
|
void pidf_reset (pidf_t *pid)
|
|
{
|
|
pid->error = 0.0f;
|
|
pid->i_error = 0.0f;
|
|
pid->d_error = 0.0f;
|
|
pid->sample_rate_prev = 1.0f;
|
|
}
|
|
|
|
float pidf (pidf_t *pid, float command, float actual, float sample_rate)
|
|
{
|
|
float error = command - actual;
|
|
/*
|
|
if(error > pid->deadband)
|
|
error -= pid->deadband;
|
|
else if (error < pid->deadband)
|
|
error += pid->deadband;
|
|
else
|
|
error = 0.0f;
|
|
*/
|
|
// calculate the proportional term
|
|
float pidres = pid->cfg.p_gain * error;
|
|
|
|
// calculate and add the integral term
|
|
pid->i_error += error * (pid->sample_rate_prev / sample_rate);
|
|
|
|
if(pid->cfg.i_max_error != 0.0f) {
|
|
if (pid->i_error > pid->cfg.i_max_error)
|
|
pid->i_error = pid->cfg.i_max_error;
|
|
else if (pid->i_error < -pid->cfg.i_max_error)
|
|
pid->i_error = -pid->cfg.i_max_error;
|
|
}
|
|
|
|
pidres += pid->cfg.i_gain * pid->i_error;
|
|
|
|
// calculate and add the derivative term
|
|
if(pid->cfg.d_gain != 0.0f) {
|
|
float p_error = (error - pid->d_error) * (sample_rate / pid->sample_rate_prev);
|
|
if(pid->cfg.d_max_error != 0.0f) {
|
|
if (p_error > pid->cfg.d_max_error)
|
|
p_error = pid->cfg.d_max_error;
|
|
else if (p_error < -pid->cfg.d_max_error)
|
|
p_error = -pid->cfg.d_max_error;
|
|
}
|
|
pidres += pid->cfg.d_gain * p_error;
|
|
pid->d_error = error;
|
|
}
|
|
|
|
pid->sample_rate_prev = sample_rate;
|
|
|
|
// limit error output
|
|
if(pid->cfg.max_error != 0.0f) {
|
|
if(pidres > pid->cfg.max_error)
|
|
pidres = pid->cfg.max_error;
|
|
else if(pidres < -pid->cfg.max_error)
|
|
pidres = -pid->cfg.max_error;
|
|
}
|
|
|
|
pid->error = pidres;
|
|
|
|
return pidres;
|
|
}
|