Jorn Dokter / Mbed 2 deprecated TEB_branch2

TEB programma

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

controller.cpp@16:fa8001fef71b, 2019-10-03 (annotated)

Committer:
JornD
Date:
Thu Oct 03 13:07:20 2019 +0000
Revision:
16:fa8001fef71b
Parent:
15:95034d92bc76
Child:
17:16d29ed4ab00
Made the controller a bit better;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JornD 15:95034d92bc76 1 //Gains
JornD 15:95034d92bc76 2 double K_p = 1;//Position gain
JornD 15:95034d92bc76 3 double K_i = 1;//Integral gain
JornD 15:95034d92bc76 4 double K_d = 1;//Diverentiation gain
JornD 15:95034d92bc76 5
JornD 15:95034d92bc76 6 static double u_i = 0;
JornD 15:95034d92bc76 7 static double e_Prev = 0;
JornD 15:95034d92bc76 8
JornD 15:95034d92bc76 9 double ControllerPID(double e, double Ts)
JornD 15:95034d92bc76 10 {
JornD 15:95034d92bc76 11 double u_p = K_p*e; //Position action
JornD 15:95034d92bc76 12 double u_i = K_i*(e*Ts+u_i); //Integral action
JornD 16:fa8001fef71b 13 double u_d = K_d*((e-e_Prev)/Ts); //Diverential action
JornD 15:95034d92bc76 14
JornD 16:fa8001fef71b 15 e_Prev = e; //Write error to previous error
JornD 15:95034d92bc76 16
JornD 15:95034d92bc76 17 return u_p + u_i + u_d;
JornD 15:95034d92bc76 18 }