Diff: main.cpp

Revision:

3:61f0fc41f3bc

Parent:

2:e3259334299e

Child:

4:8db85182a00d

--- a/main.cpp	Mon Oct 19 13:23:10 2015 +0000
+++ b/main.cpp	Mon Oct 19 14:08:50 2015 +0000
@@ -2,18 +2,8 @@
 #include "HIDScope.h"
 
 AnalogIn EMG(A0);
-HIDScope scope (3);
-
-// Standaard formule voor het biquad filter
-double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
+HIDScope scope(4);
 
-{ 
-    double v = u - a1*v1 - a2*v2;
-    double y = b0*v + b1*v1 + b2*v2;
-    v2=v1;
-    v1=v;
-    return y;
-}
 // Filter1 = High pass filter tot 20 Hz
 double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0;
 const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
@@ -28,25 +18,37 @@
 const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1;
 const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1;
 double y1, y2, y3, y4, y5, y6, y7, u1, u2, u3, u4, u5, u6, u7;
+
+// Standaard formule voor het biquad filter
+double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
+
+{ 
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2=v1;
+    v1=v;
+    return y;
+}
+
 void Filters()
 {
     u1 = EMG.read();
     //Highpass
-    y1 = u2;
-    y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0, fh1_b1, fh1_b2);
-    y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0, fh2_b1, fh2_b2);
+    y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
+    u2 = y1;
+    y2 = biquad (u2, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0*0.918885, fh2_b1*0.918885, fh2_b2*0.918885);
     //Lowpass
     u3 = y2;
+    y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0*0.165103, fl1_b1*0.165103, fl1_b2*0.165103);
     u4 = y3;
-    y3 = biquad (u3, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0, fl1_b1, fl1_b2);
-    y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0, fl2_b1, fl2_b2);
+    y4 = biquad (u4, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0*0.031617, fl2_b1*0.031617, fl2_b2*0.031617);
     // Notch
     u5 = y4;
+    y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0*1.004206, fno1_b1*1.004206, fno1_b2*1.004206);
     u6 = y5;
+    y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
     u7 = y6;
-    y5 = biquad (u5, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0, fno1_b1, fno1_b2);
-    y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
-    y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
+    y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
     scope.set (0, u1);
     scope.set (1, y2);
     scope.set (2, y4);