tralalal

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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;