test check in

2026-03-23 11:32:44 +08:00 · 2021-02-22 12:29:17 +01:00
commit cdca5ad55c
61 changed files with 20794 additions and 0 deletions
--- a/pid.c
+++ b/pid.c
@@ -0,0 +1,105 @@
+/*
+  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;
+}