File content as of revision 21:1321ff770f99:

#include "mbed.h"

double Kp = 7.5;
double Ki = 1.02;
double Kd = 10;

void PID_controller(double error1, double error2, double &u1, double &u2, const double &T)
{  
    // proportianal part
    double u1_k = Kp * error1;
    double u2_k = Kp * error2;
    
    static double error1_integral = 0;  // 
    static double error1_prev = error1; // initialization with this value only done once!
    
    static double error2_integral = 0;
    static double error2_prev = error2; // initialization with this value only done once!
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
    // integral part
    error1_integral = error1_integral + error1 * T;
    double u1_i = Ki * error1_integral;
    
    error2_integral = error2_integral + error2 * T;
    double u2_i = Ki * error2_integral;
    
    // derivative part
    double error1_derivative = (error1 - error1_prev) / T;
    double u1_d = Kd * LowPassFilter.step(error1_derivative); // apply low pass filter to remove noise due to derivative amplification
    error1_prev = error1;
    
    double error2_derivative = (error2 - error2_prev) / T;
    double u2_d = Kd * LowPassFilter.step(error2_derivative); // apply low pass filter to remove noise due to derivative amplification
    error2_prev = error2;
    
    u1 =  u1_k+u1_i+u1_d;
    u2 =  u2_k+u2_i+u2_d;
}