Diff: help_functions/PID_controller.h

Revision:

26:23b0b6fce3a8

Parent:

24:53c300d1320e

Child:

45:829a3992d689

diff -r 734a26538711 -r 23b0b6fce3a8 help_functions/PID_controller.h
--- a/help_functions/PID_controller.h	Mon Oct 29 14:45:01 2018 +0000
+++ b/help_functions/PID_controller.h	Mon Oct 29 14:53:16 2018 +0000
@@ -17,27 +17,23 @@
     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);
+    static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
     
     // 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
+    double u1_d = Kd * LowPassFilter1.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
+    double u2_d = Kd * LowPassFilter2.step(error2_derivative); // apply low pass filter to remove noise due to derivative amplification
     error2_prev = error2;
     
     u1 =  u1_k+u1_i+u1_d;