now with PID control

git-svn-id: https://svn.spreadspace.org/avr/trunk@239 aa12f405-d877-488e-9caf-2d797e2a1cc7

4 files changed, 159 insertions, 14 deletions

diff --git a/pcr-controller/Makefile b/pcr-controller/Makefile
index 7a8217f..78400f6 100644
--- a/pcr-controller/Makefile
+++ b/pcr-controller/Makefile
@@ -22,7 +22,7 @@
 

 NAME := pcr-controller

 BOARD_TYPE := teensy2

-OBJ := $(NAME).o pwm.o

+OBJ := $(NAME).o pwm.o pid_control.o

 LIBS := util led lufa-descriptor-usbserial anyio onewire ds1820

 EXTERNAL_LIBS := lufa

 RESET_FUNC := ../tools/reset_lufa_cdc

diff --git a/pcr-controller/pcr-controller.c b/pcr-controller/pcr-controller.c
index 583d054..ac064e0 100644
--- a/pcr-controller/pcr-controller.c
+++ b/pcr-controller/pcr-controller.c
@@ -37,6 +37,7 @@
 #include "ds1820.h"
 
 #include "pwm.h"
+#include "pid_control.h"
 
 #define PIN_HIGH(PORT, PIN) PORT |= (1 << PIN)
 #define PIN_LOW(PORT, PIN) PORT &= ~(1 << PIN)
@@ -57,15 +58,8 @@
 #define PELETIER_PWM_EN PINB5
 #define TOPHEAT_PIN PIND7
 
-
-
 uint8_t num_temp_sensors_ = 0;
 uint16_t raw_temp_ = 0;
-uint16_t target_temp_ = 0;
-uint16_t pid_p_ = 0;
-uint16_t pid_i_ = 0;
-uint16_t pid_d_ = 0;
-uint16_t pid_err_ = 0;
 
 void queryAndSaveTemperature(uint8_t bit_resolution)
 {
@@ -139,6 +133,25 @@ int16_t readNumber(void)
   return atoi(buffer);
 }
 
+void setPeltierCoolingDirectionPower(int16_t value)
+{
+  if (value > 255)
+    value = 255;
+  if (value < -255)
+    value = -255;
+  
+  if (value >= 0)
+  {
+    PIN_HIGH(PORTF, PELTIER_INA);
+    PIN_LOW(PORTB, PELTIER_INB);
+    pwm_set((uint8_t) value);
+  } else {
+    PIN_LOW(PORTF, PELTIER_INA);
+    PIN_HIGH(PORTB, PELTIER_INB);
+    pwm_set((uint8_t) (-1 * value)); 
+  }
+}
+
 void handle_cmd(uint8_t cmd)
 {
   switch(cmd) {
@@ -152,18 +165,18 @@ void handle_cmd(uint8_t cmd)
   case 'L': led_toggle(); break;
   case 't':
     printf("TargetTemp: ");
-    printRawTemp(target_temp_);
+    printRawTemp(pid_getTargetValue());
     printf("\r\n");
     return;
   case 'p': 
   case 'i':
   case 'd':
-    printf("PID P: %d\r\nPID I: %d\r\nPID D: %d\r\n", pid_p_, pid_i_, pid_d_);
+    pid_printVars();
     return;
-  case 'T': target_temp_ = readNumber(); break;
-  case 'P': pid_p_ = readNumber(); break;
-  case 'I': pid_i_ = readNumber(); break;
-  case 'D': pid_d_ = readNumber();  pwm_set((uint8_t) pid_d_);  break;
+  case 'T': pid_setTargetValue(readNumber()); break;
+  case 'P': pid_setP(readNumber()); break;
+  case 'I': pid_setI(readNumber()); break;
+  case 'D': pid_setD(readNumber()); break;
   case 'A': PIN_HIGH(PORTB, PUMP_PIN); break;
   case 'a': PIN_LOW(PORTB, PUMP_PIN); break;  
   case 'B': PIN_HIGH(PORTD, TOPHEAT_PIN); break;
@@ -195,6 +208,8 @@ int main(void)
   
   pwm_init();
   
+  pid_loadFromEEPROM();
+
   num_temp_sensors_ = ds1820_discover();
   
   for(;;) 
@@ -213,6 +228,11 @@ int main(void)
     queryAndSaveTemperature(11);
     
     // PID control
+    // FIXME: if we do USB Input / Output (input especially) we delay PID controll too mauch
+    //              that's bad, since the routing requires that it be called at exact intervalls
+    //              maybe we should use a interrupt routine
+
+    setPeltierCoolingDirectionPower(pid_calc(raw_temp_));
     
     anyio_task();
   }
diff --git a/pcr-controller/pid_control.c b/pcr-controller/pid_control.c
new file mode 100644
index 0000000..ce39bc1
--- /dev/null
+++ b/pcr-controller/pid_control.c
@@ -0,0 +1,109 @@
+#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_d_last_error_ = 0;
+
+uint16_t pid_target_value_ = 0;
+
+void pid_setP(int16_t p)
+{
+    pid_p_ = (int32_t) p * PID_SCALE;
+    pid_saveToEEPROM();
+}
+
+void pid_setI(int16_t i)
+{
+    pid_i_ = (int32_t) i * PID_SCALE;
+    pid_saveToEEPROM();
+}
+
+void pid_setD(int16_t d)
+{
+    pid_d_ = (int32_t) d * PID_SCALE;
+    pid_saveToEEPROM();
+}
+
+void pid_printVars(void)
+{
+    printf("PID P: %d\r\nPID I: %d\r\nPID D: %d\r\n", (int16_t) (pid_p_ / PID_SCALE), (int16_t) (pid_i_ / PID_SCALE), (int16_t) (pid_d_ / PID_SCALE));
+}
+
+void pid_setTargetValue(uint16_t v)
+{
+    pid_target_value_ = v; 
+}
+
+uint16_t pid_getTargetValue(void)
+{
+    return pid_target_value_;
+}
+
+// PRE-CONDITION: call pid_calc at exactly regular intervals !!
+int16_t pid_calc(uint16_t current_value)
+{
+    int32_t		error = pid_target_value_ - current_value;
+    // derivative
+    // instead of derivative of error we take derivative on measurement
+    // since dError/dt = - dInput/dt   (note the - )    
+    int32_t		d_measure = current_value - pid_d_last_error_;
+    pid_d_last_error_ = error;
+
+    // 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_);
+}
\ No newline at end of file
diff --git a/pcr-controller/pid_control.h b/pcr-controller/pid_control.h
new file mode 100644
index 0000000..0a8b2ad
--- /dev/null
+++ b/pcr-controller/pid_control.h
@@ -0,0 +1,16 @@
+#ifndef PID_CONTROLLER_INCLUDE_H_
+#define PID_CONTROLLER_INCLUDE_H_
+
+#include <stdio.h>
+
+void pid_setP(int16_t p);
+void pid_setI(int16_t i);
+void pid_setD(int16_t d);
+void pid_printVars(void);
+void pid_setTargetValue(uint16_t v);
+uint16_t pid_getTargetValue(void);
+int16_t pid_calc(uint16_t current_value);
+void pid_loadFromEEPROM(void);
+void pid_saveToEEPROM(void);
+
+#endif