Diff: main.cpp

Revision:

20:0ede3818e08e

Parent:

19:0a3ee31dcdb4

Child:

21:d6a46315d5f5

--- a/main.cpp	Mon Oct 12 13:47:21 2015 +0000
+++ b/main.cpp	Mon Oct 12 14:02:20 2015 +0000
@@ -88,14 +88,24 @@
 }
 
 //Input error and Kp, Kd, Ki, output control signal
-void pid()
+double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
 {
+
+    // Derivative
+    double e_der = (error − e_prev)/CONTROL_RATE;
+    e_der = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 );
+    e_prev = e;
+    // Integral
+    e_int = e_int + CONTROL_RATE ∗ error;
+    // PID
+    return Kp ∗ e + Ki ∗ e_int + Kd ∗ e_der;
+ 
 }
 
-//Analyse filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors 
+//Analyze filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors 
 void control()
 {
-    //analyse emg (= velocity, averages?)
+    //analyze emg (= velocity, averages?)
     
     //calculate reference position based on the average emg (integrate)