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;