Jorn Dokter / Mbed 2 deprecated TEB_branch2

TEB programma

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

controller.cpp@24:d0ca537913c2, 2019-10-09 (annotated)

Committer:
JornD
Date:
Wed Oct 09 07:11:45 2019 +0000
Revision:
24:d0ca537913c2
Parent:
17:16d29ed4ab00
Child:
29:8d588d554779
Child:
40:82addb417220
Fixed L6312W error by adding inclusion guard;

Who changed what in which revision?

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