File content as of revision 2:f68fd7b1c655:

#include "PID_controler.h"
#include "mbed.h"
#include "BiQuad.h"
#include <math.h>
#include <stdio.h>
#include <iostream>
#include <stdlib.h>
#include <ctime>
#include <QEI.h>

double PID_controller(double error, double Kp, double Ki, double Kd, double Ts)
{
    static double error_integral = 0;
    static double error_prev = error;
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

    //proportional part
    double u_k = Kp * error;

    //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;
    return ((u_k + u_i + u_d)/5000);
}