Biorobotics / Robot-Software

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed Servo

Diff: help_functions/PID_controller.h

Revision:
22:31065a83d9e8
Parent:
20:31876566d70f
Parent:
17:1f93c83e211f
Child:
24:53c300d1320e
--- a/help_functions/PID_controller.h	Fri Oct 26 12:59:52 2018 +0000
+++ b/help_functions/PID_controller.h	Mon Oct 29 12:21:26 2018 +0000
@@ -5,7 +5,7 @@
 double Kd = 10;
 extern double samplingfreq = 1000;
 
-void PID_controller(double error1, double error2, double &u1, double &u2)
+void PID_controller(double error1, double error2, double &u1, double &u2, const double &T)
 {  
     double u_k = Kp * error1;
     
@@ -13,10 +13,10 @@
     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 + error1 * 1/samplingfreq;
+    error_integral = error_integral + error1 * T;
     double u_i = Ki * error_integral;
     
-    double error_derivative = (error1 - error_prev)*samplingfreq;
+    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;