![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
tralalal
Diff: main.cpp
- Revision:
- 0:297b00643040
--- /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