tralalal
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 00003 double Kp = 17.5; 00004 double Ki = 1.02; 00005 double Kd = 23.2; 00006 double Ts = 0.005f; 00007 // Sample time in seconds= tickertijd 00008 00009 double PID_controller(double error) 00010 { 00011 static double error_integral = 0; 00012 static double error_prev = error; // initialization with this value only done once! 00013 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //to eliminate sensor noise to get accurate D-action calculation 00014 00015 // Proportional part: 00016 double u_k = Kp * e; 00017 00018 // Integral part: 00019 error_integral = error_integral + error * Ts; 00020 double u_i = Ki * error_integral; 00021 00022 // Derivative part: 00023 double error_derivative = (error - error_prev)/Ts; 00024 double filtered_error_derivative = LowPassFilter.step(error_derivative); 00025 double u_d = Kd * filtered_error_derivative; error_prev = error; 00026 error_prev = error; 00027 00028 // Sum all parts and return it 00029 return u_k + u_i + u_d;
Generated on Wed Jul 13 2022 04:37:05 by
1.7.2