Diff: help_functions/PID_controller.h

Branch:

bla

Revision:

21:1321ff770f99

Parent:

17:1f93c83e211f

diff -r 1f93c83e211f -r 1321ff770f99 help_functions/PID_controller.h
--- a/help_functions/PID_controller.h	Thu Oct 25 22:01:12 2018 +0000
+++ b/help_functions/PID_controller.h	Fri Oct 26 15:20:57 2018 +0000
@@ -6,18 +6,33 @@
 
 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;
+    
+    error2_integral = error2_integral + error2 * T;
+    double u2_i = Ki * error2_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;
+    // 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