1 files changed, 137 insertions, 0 deletions
diff --git a/pcr-controller/pid_control.c b/pcr-controller/pid_control.c
new file mode 100644
index 0000000..6ab275b
--- /dev/null
+++ b/pcr-controller/pid_control.c
@@ -0,0 +1,137 @@
+/*
+ *  r3PCR Teensy Controller Code
+ *
+ *
+ *  Copyright (C) 2013-2014 Bernhard Tittelbach <xro@realraum.at>
+ *
+ *  This code 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
+ *  any later version.
+ *
+ *  This code 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 spreadspace avr utils. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "pid_control.h"
+
+#include <avr/eeprom.h>
+
+EEMEM int32_t pid_p_eeprom_;
+EEMEM int32_t pid_i_eeprom_;
+EEMEM int32_t pid_d_eeprom_;
+EEMEM int32_t crc_eeprom_;
+
+#define PID_SCALE 1024L
+
+int32_t pid_outlimit_min_ = -255 * PID_SCALE;
+int32_t pid_outlimit_max_ = 255 * PID_SCALE;
+
+int32_t pid_p_ = 8192;
+int32_t pid_i_ = 512;
+int32_t pid_d_ = 24576;
+
+int32_t pid_i_integralsum_ = 0;
+int32_t pid_last_input_ = 0;
+
+int16_t pid_target_value_ = PID_DISABLED;
+
+void pid_setP(int16_t p)
+{
+    pid_p_ = (int32_t) p;
+    pid_saveToEEPROM();
+}
+
+void pid_setI(int16_t i)
+{
+    pid_i_ = (int32_t) i;
+    pid_saveToEEPROM();
+}
+
+void pid_setD(int16_t d)
+{
+    pid_d_ = (int32_t) d;
+    pid_saveToEEPROM();
+}
+
+void pid_printVars(void)
+{
+    printf("{\"PID_P\":%d, \"PID_I\":%d, \"PID_D\":%d, \"PID_SCALE\":%d}\r\n", (int16_t) (pid_p_), (int16_t) (pid_i_), (int16_t) (pid_d_), (int16_t) PID_SCALE);
+}
+
+void pid_setTargetValue(int16_t v)
+{
+    pid_target_value_ = v;
+}
+
+int16_t pid_getTargetValue(void)
+{
+    return pid_target_value_;
+}
+
+int pid_isEnabled(void)
+{
+    return pid_target_value_ != PID_DISABLED;
+}
+
+// PRE-CONDITION: call pid_calc at exactly regular intervals !!
+int16_t pid_calc(int16_t current_value)
+{
+    if (pid_target_value_ == PID_DISABLED)
+        return PID_DISABLED;
+
+    int32_t		error = (int32_t) pid_target_value_ - (int32_t) current_value;
+    // derivative
+    // instead of derivative of error we take derivative on measurement
+    // since dError/dt = - dInput/dt   (note the - )
+    int32_t		d_measure = (int32_t) current_value - pid_last_input_;
+    pid_last_input_ = (int32_t) current_value;
+
+    // integral (bring pid_i_ into integral, so we get smooth transfer if pid_i_ suddenly changes)
+    pid_i_integralsum_ += pid_i_ * error;
+
+    // prevent integrator wind-up
+    if (pid_i_integralsum_ > pid_outlimit_max_)
+        pid_i_integralsum_ = pid_outlimit_max_;
+    else if (pid_i_integralsum_ < pid_outlimit_min_)
+        pid_i_integralsum_ = pid_outlimit_min_;
+
+    // combine factors (insofar as we did not already do it above)
+    int32_t pid_output_preclamp = (
+        (error * pid_p_) + (pid_i_integralsum_) - (d_measure * pid_d_)
+    );
+
+    // limit output
+    if (pid_output_preclamp > pid_outlimit_max_)
+        return (int16_t) (pid_outlimit_max_ / PID_SCALE);
+    else if (pid_output_preclamp < pid_outlimit_min_)
+        return (int16_t) (pid_outlimit_min_  / PID_SCALE);
+    else
+        return (int16_t) (pid_output_preclamp  / PID_SCALE);
+}
+
+void pid_loadFromEEPROM(void)
+{
+    int32_t p = eeprom_read_dword((uint32_t *) &pid_p_eeprom_);
+    int32_t i = eeprom_read_dword((uint32_t *) &pid_i_eeprom_);
+    int32_t d = eeprom_read_dword((uint32_t *) &pid_d_eeprom_);
+    int32_t crc = eeprom_read_dword((uint32_t *) &crc_eeprom_);
+    if (crc == (p ^ i ^ d)) {
+        pid_p_ = p;
+        pid_i_ = i;
+        pid_d_ = d;
+    }
+}
+
+void pid_saveToEEPROM(void)
+{
+    eeprom_write_dword((uint32_t *) &pid_p_eeprom_, pid_p_);
+    eeprom_write_dword((uint32_t *) &pid_i_eeprom_, pid_i_);
+    eeprom_write_dword((uint32_t *) &pid_d_eeprom_, pid_d_);
+    eeprom_write_word((uint16_t *) &crc_eeprom_,  pid_p_ ^ pid_i_ ^ pid_d_);
+}