mirror of
https://github.com/grblHAL/core.git
synced 2026-02-06 18:29:49 +08:00
64c35930c7
Added experimental support for G66 (modal macro call) and G67 (end modal macro call). Made axis letter to axis/motor assignment for axes ABCUVW freely changeable at compile time. Fix for some G65 arguments being incorrectly validated for normal use (sign, range). Added repeat support to G65 macro call via the optional L parameter word. Changed default setting for ABC-axes to rotary. Changed defaults for jerk settings to 10x acceleration settings. Disabled jerk for jog, probe and spindle synchronized motion. Added _active_probe system parameter, returns -1 if no probe inputs available. Minor bug fix, G5.1 and G33.1 motion commands were not coverted to the correct string equivalent in $G output.
106 lines
2.9 KiB
C
106 lines
2.9 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 grblHAL code
|
|
|
|
Part of grblHAL
|
|
|
|
Copyright (c) 2020-2021 Terje Io
|
|
|
|
grblHAL 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.
|
|
|
|
grblHAL 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 grblHAL. 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;
|
|
}
|