Jorn Dokter / Mbed 2 deprecated TEB_branch2

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

Diff: controller.cpp

Revision:
55:f764f223c56a
Parent:
48:39f84359998a
--- a/controller.cpp	Mon Oct 14 20:05:57 2019 +0000
+++ b/controller.cpp	Tue Oct 15 12:19:22 2019 +0000
@@ -62,4 +62,13 @@
         float PA[] = {1,2,3};
         float QA[] = {4,5,6};
         BuildController(Set_EndAffector, PA, QA);
+}
+
+float DigitalBiquadFilter(ControllerSettings& StrucName, MemoryIO& MemName, 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;        
+    
+    
+    return Y; 
 }
\ No newline at end of file