Diff: help_functions/PID_controller.h

Revision:

45:829a3992d689

Parent:

26:23b0b6fce3a8

Child:

46:9b60a3c1acab

--- a/help_functions/PID_controller.h	Thu Nov 01 12:39:24 2018 +0000
+++ b/help_functions/PID_controller.h	Thu Nov 01 19:48:22 2018 +0000
@@ -1,41 +1,89 @@
 #include "mbed.h"
 
-double Kp = 7.5;
-double Ki = 1.02;
-double Kd = 10;
+double Kp = 0.1;
+double Ki = 0.4;
+double Kd = 4;
 extern double samplingfreq = 1000;
 
-void PID_controller(double error1, double error2, double &u1, double &u2, const double &T)
-{  
-    // proportianal part
-    double u1_k = Kp * error1;
-    double u2_k = Kp * error2;
-    
-    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!
+//void PID_controller(double error1, double error2, double &u1, double &u2, const double &T)
+//{  
+//    // proportianal part
+//    double u1_k = Kp * error1;
+//    double u2_k = Kp * error2;
+//    
+//    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 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;
+//    
+//    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 * 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 * 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;
+//    u2 =  u2_k+u2_i+u2_d;
+//}
+void PID_controller(double error1, double error2, double &u1, double &u2, double T)
+{   
+    static double error_prev1 = error1; // initialization with this value only done once!
+    static double error_prev2 = error2; // initialization with this value only done once!
+    double u_pid2, u_pid1;
+    static double error_integral1, error_integral2 = 0;
+  
+    double u_k1 = Kp * error1;
+    double u_k2 = Kp * error2;
     
     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_derivative1 = (error1 - error_prev1)*T;
+    double filtered_error_derivative1 = LowPassFilter1.step(error_derivative1);
+    double u_d1 = Kd * filtered_error_derivative1;
     
-    error2_integral = error2_integral + error2 * T;
-    double u2_i = Ki * error2_integral;
+    double error_derivative2 = (error2 - error_prev2)*T;
+    double filtered_error_derivative2 = LowPassFilter2.step(error_derivative2);
+    double u_d2 = Kd * filtered_error_derivative2;
     
-    // derivative part
-    double error1_derivative = (error1 - error1_prev) / T;
-    double u1_d = Kd * LowPassFilter1.step(error1_derivative); // apply low pass filter to remove noise due to derivative amplification
-    error1_prev = error1;
+    error_integral1 = error_integral1 +( error1 * T);
+    error_integral2 = error_integral2 +( error2 * T);
+    if (error_integral1 > 1){
+        error_integral1 = 1;
+    }
+    else if (error_integral1 < -1){
+        error_integral1 = -1;
+    }
+    if (error_integral2 > 1){
+        error_integral2 = 1;
+    }
+    else if (error_integral2 < -1){
+        error_integral2 = -1;
+    }
     
-    double error2_derivative = (error2 - error2_prev) / T;
-    double u2_d = Kd * LowPassFilter2.step(error2_derivative); // apply low pass filter to remove noise due to derivative amplification
-    error2_prev = error2;
+    double u_i1 = Ki*error_integral1;
+    double u_i2 = Ki*error_integral2;
+    
+    u_pid1 = u_k1 + u_d1 + u_i1;
+    u_pid2 = u_k2 + u_d2 + u_i2;
     
-    u1 =  u1_k+u1_i+u1_d;
-    u2 =  u2_k+u2_i+u2_d;
+    u1 = -u_pid1;
+    u2 = -u_pid2;
+    
+    error_prev1 = error1;
+    error_prev2 = error2;       
 }
\ No newline at end of file