Diff: help_functions/PID_controller.h

Revision:

24:53c300d1320e

Parent:

22:31065a83d9e8

Child:

26:23b0b6fce3a8

--- a/help_functions/PID_controller.h	Mon Oct 29 12:32:39 2018 +0000
+++ b/help_functions/PID_controller.h	Mon Oct 29 12:38:04 2018 +0000
@@ -7,18 +7,39 @@
 
 void PID_controller(double error1, double error2, double &u1, double &u2, const double &T)
 {  
-    double u_k = Kp * error1;
+    // proportianal part
+    double u1_k = Kp * error1;
+    double u2_k = Kp * error2;
     
-    static double error_integral = 0;
-    static double error_prev = error1; // initialization with this value only done once!
+    static double error1_integral = 0;  // 
+    static double error1_prev = error1; // initialization with this value only done once!
+    
+    static double error2_integral = 0;
+    static double error2_prev = error2; // initialization with this value only done once!
+    
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
     
-    error_integral = error_integral + error1 * T;
-    double u_i = Ki * error_integral;
+    // integral part
+    error1_integral = error1_integral + error1 * T;
+    double u1_i = Ki * error1_integral;
     
     double error_derivative = (error1 - error_prev) / T;
     double filtered_error_derivative = LowPassFilter.step(error_derivative);
     double u_d = Kd * filtered_error_derivative;
     error_prev = error1;
     u1 =  u_k+u_i+u_d;
+    error2_integral = error2_integral + error2 * T;
+    double u2_i = Ki * error2_integral;
+    
+    // derivative part
+    double error1_derivative = (error1 - error1_prev) / T;
+    double u1_d = Kd * LowPassFilter.step(error1_derivative); // apply low pass filter to remove noise due to derivative amplification
+    error1_prev = error1;
+    
+    double error2_derivative = (error2 - error2_prev) / T;
+    double u2_d = Kd * LowPassFilter.step(error2_derivative); // apply low pass filter to remove noise due to derivative amplification
+    error2_prev = error2;
+    
+    u1 =  u1_k+u1_i+u1_d;
+    u2 =  u2_k+u2_i+u2_d;
 }
\ No newline at end of file