Diff: help_functions/PID_controller.h

Revision:

10:7339dca7d604

Parent:

1:f1b8f0462e2a

Child:

17:1f93c83e211f

Child:

20:31876566d70f

diff -r 8ff9566c91e2 -r 7339dca7d604 help_functions/PID_controller.h
--- a/help_functions/PID_controller.h	Mon Oct 22 19:13:37 2018 +0000
+++ b/help_functions/PID_controller.h	Tue Oct 23 08:44:17 2018 +0000
@@ -5,21 +5,20 @@
 double Kd = 10;
 double samplingfreq = 1000;
 
-double PID_controller(double error)
-{
-    
-    double u_k = Kp * error;
+void PID_controller(double error1, double error2, double &u1, double &u2)
+{  
+    double u_k = Kp * error1;
     
     static double error_integral = 0;
-    static double error_prev = error; // initialization with this value only done once!
+    static double error_prev = error1; // initialization with this value only done once!
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
     
-    error_integral = error_integral + error * 1/samplingfreq;
+    error_integral = error_integral + error1 * 1/samplingfreq;
     double u_i = Ki * error_integral;
     
-    double error_derivative = (error - error_prev)*samplingfreq;
+    double error_derivative = (error1 - error_prev)*samplingfreq;
     double filtered_error_derivative = LowPassFilter.step(error_derivative);
     double u_d = Kd * filtered_error_derivative;
-    error_prev = error;
-    return u_k+u_i+u_d;
+    error_prev = error1;
+    u1 =  u_k+u_i+u_d;
 }
\ No newline at end of file