Diff: main.cpp

Revision:

4:8db85182a00d

Parent:

3:61f0fc41f3bc

Child:

5:e78295b978ab

--- a/main.cpp	Mon Oct 19 14:08:50 2015 +0000
+++ b/main.cpp	Tue Oct 20 10:38:37 2015 +0000
@@ -1,7 +1,10 @@
 #include "mbed.h"
 #include "HIDScope.h"
 
-AnalogIn EMG(A0);
+AnalogIn EMG_bicepsleft(A0);
+AnalogIn EMG_bicepsright (A1);
+AnalogIn EMG_legleft (A2);
+AnalogIn EMG_legright (A3); 
 HIDScope scope(4);
 
 // Filter1 = High pass filter tot 20 Hz
@@ -17,6 +20,10 @@
 const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1;
 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;
+// Filter4 = Lowpass filter at 5 Hz
+double flp1_v1=0, flp1_v2=0, flp2_v1=0, flp2_v2=0;
+const double flp1_a1=-0.97922725527, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
+const double flp2_a1=-1.97879353121, flp2_a2=0.97922951943, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
 double y1, y2, y3, y4, y5, y6, y7, u1, u2, u3, u4, u5, u6, u7;
 
 // Standaard formule voor het biquad filter
@@ -30,9 +37,9 @@
     return y;
 }
 
-void Filters()
+void Filters_bicepleft()
 {
-    u1 = EMG.read();
+    u1 = EMG_bicepsleft.read();
     //Highpass
     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;
@@ -49,12 +56,21 @@
     y6 = biquad (u6, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
     u7 = y6;
     y7 = biquad (u7, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0*0.973227, fno3_b1*0.973227, fno3_b2*0.973227);
+    // Rectify sample 
+    y8 = fabs(y7);
+    // Make it smooth 
+    u8 = y8; 
+    y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
+    u9 = y9;
+    final_filter = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
+    // Send it to scope 
     scope.set (0, u1);
-    scope.set (1, y2);
-    scope.set (2, y4);
-    scope.set (3, y7); 
+    scope.set (1, y7); 
+    scope.set (2, y8);
+    scope.set (3, final_filter);
     scope.send ();
     } 
+    
 Ticker filter;
 int main ()
 {filter.attach_us(Filters,1e3);}