Jorn Dokter / Mbed 2 deprecated TEB_branch2

TEB programma

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

Diff: controller.cpp

Revision:
16:fa8001fef71b
Parent:
15:95034d92bc76
Child:
17:16d29ed4ab00
--- a/controller.cpp	Thu Oct 03 13:03:38 2019 +0000
+++ b/controller.cpp	Thu Oct 03 13:07:20 2019 +0000
@@ -10,9 +10,9 @@
 {
     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
+    double u_d = K_d*((e-e_Prev)/Ts);   //Diverential action
     
-    e_Prev = e;
+    e_Prev = e; //Write error to previous error
     
     return u_p + u_i + u_d;
 }
\ No newline at end of file