Diff: main.cpp

Revision:

0:297b00643040

diff -r 000000000000 -r 297b00643040 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 30 18:57:03 2018 +0000
@@ -0,0 +1,29 @@
+#include "mbed.h"
+
+double Kp = 17.5;
+double Ki = 1.02;
+double Kd = 23.2;
+double Ts = 0.005f;
+ // Sample time in seconds= tickertijd
+
+double PID_controller(double error)
+{
+    static double error_integral = 0;
+    static double error_prev = error; // initialization with this value only done once!  
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //to eliminate sensor noise to get accurate D-action calculation
+    
+    // Proportional part:  
+    double u_k = Kp * e;
+    
+    // Integral part:
+    error_integral = error_integral + error * Ts;
+    double u_i = Ki * error_integral;
+    
+    // Derivative part: 
+    double error_derivative = (error - error_prev)/Ts;
+    double filtered_error_derivative = LowPassFilter.step(error_derivative);
+    double u_d = Kd * filtered_error_derivative;  error_prev = error;
+    error_prev = error;
+    
+    // Sum all parts and return it  
+    return u_k + u_i + u_d;   
\ No newline at end of file