![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
tralalal
main.cpp
- Committer:
- Marle
- Date:
- 2018-10-30
- Revision:
- 0:297b00643040
File content as of revision 0:297b00643040:
#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;