File content as of revision 45:829a3992d689:

#include "mbed.h"

double Kp = 0.1;
double Ki = 0.4;
double Kd = 4;
extern double samplingfreq = 1000;

//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 LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
//    static BiQuad LowPassFilter2(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 * LowPassFilter1.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 * LowPassFilter2.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;
//}
void PID_controller(double error1, double error2, double &u1, double &u2, double T)
{   
    static double error_prev1 = error1; // initialization with this value only done once!
    static double error_prev2 = error2; // initialization with this value only done once!
    double u_pid2, u_pid1;
    static double error_integral1, error_integral2 = 0;
  
    double u_k1 = Kp * error1;
    double u_k2 = Kp * error2;
    
    static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
    
    double error_derivative1 = (error1 - error_prev1)*T;
    double filtered_error_derivative1 = LowPassFilter1.step(error_derivative1);
    double u_d1 = Kd * filtered_error_derivative1;
    
    double error_derivative2 = (error2 - error_prev2)*T;
    double filtered_error_derivative2 = LowPassFilter2.step(error_derivative2);
    double u_d2 = Kd * filtered_error_derivative2;
    
    error_integral1 = error_integral1 +( error1 * T);
    error_integral2 = error_integral2 +( error2 * T);
    if (error_integral1 > 1){
        error_integral1 = 1;
    }
    else if (error_integral1 < -1){
        error_integral1 = -1;
    }
    if (error_integral2 > 1){
        error_integral2 = 1;
    }
    else if (error_integral2 < -1){
        error_integral2 = -1;
    }
    
    double u_i1 = Ki*error_integral1;
    double u_i2 = Ki*error_integral2;
    
    u_pid1 = u_k1 + u_d1 + u_i1;
    u_pid2 = u_k2 + u_d2 + u_i2;
    
    u1 = -u_pid1;
    u2 = -u_pid2;
    
    error_prev1 = error1;
    error_prev2 = error2;       
}