Jorn Dokter / Mbed 2 deprecated TEB_branch2

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

controller.cpp

Committer:
JornD
Date:
2019-10-09
Revision:
24:d0ca537913c2
Parent:
17:16d29ed4ab00
Child:
29:8d588d554779
Child:
40:82addb417220

File content as of revision 24:d0ca537913c2:

//Gains
double e = 1.02; 
double Ts = 0.92;

    double K_p = 1;//Position gain
    double K_i = 1;//Integral gain
    double K_d = 1;//Diverentiation gain
    
    //static double u_i = 0;
    static double e_Prev = 0;

double ControllerPID(double e, double Ts)
{
    double u_p = K_p*e;               //Position action
    double u_i = K_i*(e*Ts+u_i);      //Integral action
    double u_d = K_d*((e-e_Prev)/Ts);   //Diverential action
    
    e_Prev = e; //Write error to previous error
    
    double u;
    
    u = u_p + u_i + u_d;
    
    return u;
}