From e018a75cd3feb9696a462b428e7df5323f38bccb Mon Sep 17 00:00:00 2001
From: id101010 <aaron@duckpond.ch>
Date: Tue, 2 Jun 2015 16:25:45 +0200
Subject: [PATCH] Implemented basic pi and pid controller

---
 common/app/pixy_control.c | 84 +++++++++++++++++++++++++++++++++++++++
 common/app/pixy_control.h | 12 ++++++
 2 files changed, 96 insertions(+)
 create mode 100644 common/app/pixy_control.c
 create mode 100644 common/app/pixy_control.h

diff --git a/common/app/pixy_control.c b/common/app/pixy_control.c
new file mode 100644
index 0000000..fef3fc2
--- /dev/null
+++ b/common/app/pixy_control.c
@@ -0,0 +1,84 @@
+/*
+ * pixy_control.c
+ *
+ * Notation
+ * --------
+ *
+ * x    :   Sollwert (Führgrösse)
+ * w    :   Istwert (Reglergrösse)
+ * esum :   Integralteil
+ * e    :   Regelabweichung
+ * y    :   Stellgrösse
+ *
+ *
+ */
+#include<pixy_control.h>
+
+// PID tuning factors
+#define REG_PID_KP      1
+#define REG_PID_KI      1
+#define REG_PID_KD      1
+#define REG_PID_TA      1
+
+#define REG_PI_KP       1
+#define REG_PI_KI       1
+#define REG_PI_TA       1
+
+void int_init(void){  
+    // TODO Init ports and outputs if needed.
+}
+
+// PI controller
+uint16_t pixy_PI(uint16_t x, uint16_t w)
+{
+    uint16_t        y = 0;
+    float           e = 0;
+    static float    esum;
+
+    // Calculate controller offset
+    e = x - w;
+
+    //----PID-control--------------------------------
+
+    // Integrate and check boundaries if necassary
+    esum = esum + e;
+    //esum = (esum < -400) ? -400 : esum;
+    //esum = (esum > 400)  ?  400 : esum;
+
+    // PI controller equation
+    y = ( REG_PI_KP * e ) + ( REG_PI_KI * REG_PI_TA * esum);
+
+    //-----------------------------------------------
+
+    return y;
+}
+
+// PID controller
+uint16_t control_SpeedPID(uint16_t x, uint16_t w)
+{
+    float e;
+    static float esum;
+    static float ealt;
+    uint16_t y;
+
+    // Calculate controller offset
+    e = x - w;
+    
+    //----PID-control--------------------------------
+    
+    // Integrate and check boundaries if necessary
+    esum = esum + e;
+    //esum = (esum > 50)? 50 : esum;
+    //esum = (esum < -50)? -50 : esum;
+
+    // PID controller equation
+    y = REG_PID_KP * e + REG_PID_KI * REG_PID_TA * esum + REG_PID_KD * (e - ealt)/REG_PID_TA;
+    
+    //-----------------------------------------------
+
+    // save old value
+    ealt = e;
+
+    return y;
+}
+
diff --git a/common/app/pixy_control.h b/common/app/pixy_control.h
new file mode 100644
index 0000000..da895e7
--- /dev/null
+++ b/common/app/pixy_control.h
@@ -0,0 +1,12 @@
+/* 
+ * pixy_control.h
+ */
+
+#ifndef _CONTROL_H_
+#define _CONTROL_H_
+
+void int_init(void);
+uint16_t pixy_PID(uint16_t x, uint16_t w);
+uint16_t pixy_PI(uint16_t x, uint16_t w);
+
+#endif