Diff: main.cpp

Revision:

6:30698d9be7f8

Parent:

5:e78295b978ab

Child:

7:d6416ed1672c

--- a/main.cpp	Tue Oct 20 10:45:22 2015 +0000
+++ b/main.cpp	Tue Oct 20 12:40:29 2015 +0000
@@ -4,8 +4,9 @@
 AnalogIn EMG_bicepsleft(A0);
 AnalogIn EMG_bicepsright (A1);
 AnalogIn EMG_legleft (A2);
-AnalogIn EMG_legright (A3); 
+AnalogIn EMG_legright (A3);
 HIDScope scope(4);
+Timer cali;
 
 // Filter1 = High pass filter tot 20 Hz
 double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0;
@@ -24,12 +25,37 @@
 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;
+double y1, y2, y3, y4, y5, y6, y7, y8, y9, u1, u2, u3, u4, u5, u6, u7,u8 , u9 ;
+double final_filter1, final_filter2, final_filter3, final_filter4;
+double fblmax = 0;
+double fbrmax = 0;
+double fllmax = 0;
+double flrmax = 0;
+
+void Calibrations ()
+{
+    cali.start();
+    while (cali.read()<= 5) {
+        if (final_filter1 >= fblmax) {
+            fblmax=final_filter1;
+        }
+        if (final_filter2 >= fbrmax) {
+            fbrmax = final_filter2;
+        }
+        if (final_filter3 >= fllmax) {
+            fllmax=final_filter3;
+        }
+        if (final_filter4 >= flrmax) {
+            flrmax=final_filter4;
+        }
+    }
+}
+
 
 // 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;
@@ -56,17 +82,16 @@
     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 
+    // Rectify sample
     y8 = fabs(y7);
-    // Make it smooth 
-    u8 = y8; 
+    // 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);
-    }
-    
-    
-    void Filters_bicepsright()
+    final_filter1 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
+}
+
+void Filters_bicepsright()
 {
     u1 = EMG_bicepsright.read();
     //Highpass
@@ -85,16 +110,15 @@
     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 
+    // Rectify sample
     y8 = fabs(y7);
-    // Make it smooth 
-    u8 = y8; 
+    // 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);
-    }
-    
-    void Filters_legleft()
+    final_filter2 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
+}
+void Filters_legleft()
 {
     u1 = EMG_legleft.read();
     //Highpass
@@ -113,16 +137,15 @@
     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 
+    // Rectify sample
     y8 = fabs(y7);
-    // Make it smooth 
-    u8 = y8; 
+    // 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);
-    }
-    
-    void Filters_legright()
+    final_filter3 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
+}
+void Filters_legright()
 {
     u1 = EMG_legright.read();
     //Highpass
@@ -141,25 +164,29 @@
     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 
+    // Rectify sample
     y8 = fabs(y7);
-    // Make it smooth 
-    u8 = y8; 
+    // 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);
-    }
+    final_filter4 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
+}
 // Send signals to HIDScope
-void Filters_EMG {
-    scope.set (0,Filters_bicepsleft);
-    scope.set (1,Filters_bicepsright);
-    scope.set (2,Filters_legleft);
-    scope.set (3,Filters_legright);
+void Filters_EMG ()
+{
+    scope.set (0,final_filter1);
+    scope.set (1,final_filter2);
+    scope.set (2,final_filter3);
+    scope.set (3,final_filter4);
     scope.send ();
-    }
+}
+
 Ticker filter;
 int main ()
-{filter.attach_us(Filters_EMG,1e3);}
+{
+    filter.attach_us(Filters_EMG,1e3);
+}