Diff: controller.cpp

Revision:

47:adbb886ed3cd

Parent:

43:9579a1afe9cb

Child:

48:39f84359998a

--- a/controller.cpp	Mon Oct 14 14:06:39 2019 +0000
+++ b/controller.cpp	Mon Oct 14 14:50:15 2019 +0000
@@ -1,25 +1,30 @@
 #include "structures.h"
-
-//Vectors of Numerators and Denumerators of transferfunctions
+#include <math.h>
 
-//Hier het omschrijven van TF(s) naar direct (in z)
-float TransferToDirect(float P, float Q)
+float * Filt(float P[], float Q[])
 {
-    
-    return Coeff;
-}
-
+    extern float Ts;
+    float K = 2/Ts;
+    static float coefficients[5];
+        coefficients[0] = (Q[0] * pow(K,2) + Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
+        coefficients[1] = (2 * Q[2] - 2 * Q[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
+        coefficients[2] = (Q[0] * pow(K,2) - Q[1] * K + Q[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
+        coefficients[3] = (2 * P[2] - 2 * P[0] * pow(K,2))/(P[0] * pow(K,2) + P[1] * K + P[2]);
+        coefficients[4] = (P[0] * pow(K,2) - P[1] * K + P[2])/(P[0] * pow(K,2) + P[1] * K + P[2]);
+    return coefficients;
+} 
 
 void InitializeControllers(void)
 {
     //Define Controller structures, shorthand: Set_
-    ControllerSettings Set_EMGProcessing;
+    ControllerSettings Set_ProcessEMG;
     
     //Define Memory cells, shorthand: Mem_
     MemoryIO MemOne;
     
     //Write controller setting values to structures
-    float CoeffOne(P,Q);
+    float *CoeffOne[];
+    float CoeffOne = Filt(P[],Q[]);
     
     SetOne.A = CoeffOne[0];
     SetOne.B = CoeffOne[1];