Diff: control.cpp

Revision:

0:f3b355df6f26

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/control.cpp	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,55 @@
+#include "control.h"
+#include "threads.h"
+
+cControl::cControl() {
+    K[0] = 9.6; K[1] = 2.3; K[2] = -0.12; K[3] = -0.2;
+    q[0] = 24;
+    q[1] = -39.999;
+    q[2] = 16;
+    e[0] = 0;
+    e[1] = 0;
+    e[2] = 0;
+    u_old = 0;
+}
+
+void cControl::setCurrent() {
+    float temp = 0;
+    if(programMode == RUNNING) {
+        /* STATE FEEDBACK */
+        if(controller == ST_FEEDBACK) {
+            temp = -K[0]*states.phi1 - K[1]*states.omega1 - K[2]*states.phi2 - K[3]*states.omega2;
+            
+            if(temp > 10.0)
+                states.current = 10.0;
+            
+            else {
+                
+                if(temp < -10.0)
+                    states.current = -10.0;
+                
+                else
+                    states.current = temp;
+            }
+        }
+        /* PID CONTROLLER */
+        else {   
+            float temp;
+            e[0] = -states.phi1;
+            temp = u_old  + q[0]*e[0] + q[1]*e[1] + q[2]*e[2];
+            if(temp > 10.0)
+                   states.current = 10.0;
+            else {
+                if(temp < -10.0)
+                    states.current = -10.0;
+                else
+                    states.current = temp;
+            }
+            
+            e[1] = e[0];
+            e[2] = e[1];
+            u_old = states.current;
+        }
+    }
+    else
+        states.current = 0.0;
+}
\ No newline at end of file