Jorn Dokter / Mbed 2 deprecated TEB_branch2

TEB programma

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

controller.cpp

Committer:
JornD
Date:
2019-10-03
Revision:
16:fa8001fef71b
Parent:
15:95034d92bc76
Child:
17:16d29ed4ab00

File content as of revision 16:fa8001fef71b:

//Gains
    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
    
    return u_p + u_i + u_d;
}