Jorn Dokter / Mbed 2 deprecated TEB_branch2

TEB programma

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

Diff: controller.cpp

Branch:
Branch2
Revision:
62:44f5c366110b
Parent:
61:4c7de1e2f9fe
Child:
64:5a6bf0cd1c50
--- a/controller.cpp	Wed Oct 16 09:24:22 2019 +0000
+++ b/controller.cpp	Wed Oct 16 10:17:23 2019 +0000
@@ -63,7 +63,7 @@
 float Biquad(ControllerSettings& StrucName, MemoryIO& MemName, float X)
 {
     //Difference equation which implements the digital biquad filter
-    float Y = StrucName.A * X + StrucName.B * MemName.Xm + StrucName.C * MemName.Xmm + StrucName.D * MemName.Ym + StrucName.E * MemName.Ymm;        
+    float Y = StrucName.A * X + StrucName.B * MemName.Xm + StrucName.C * MemName.Xmm - StrucName.D * MemName.Ym - StrucName.E * MemName.Ymm;        
     
     MemName.ShiftValues(X,Y);