Diff: main.cpp

Revision:

24:6661dd6a1718

Parent:

23:624827baffa6

Child:

25:5e9cc994cd1f

--- a/main.cpp	Thu Oct 08 19:15:14 2015 +0000
+++ b/main.cpp	Mon Oct 12 09:24:44 2015 +0000
@@ -2,6 +2,7 @@
 #include "HIDScope.h"
 #include "MODSERIAL.h"
 #include "biquadFilter.h"
+#include "MAF.h"
 
 //Define objects
     AnalogIn    EMG_left(A0); //Analog input
@@ -9,7 +10,8 @@
     Ticker      SampleEMG;
     Ticker      ScopeTimer;
     Ticker      serial;
-    HIDScope    scope(4);
+    Ticker      MovingAverage;
+    HIDScope    scope(6);
     DigitalOut led(LED_RED);
     MODSERIAL pc(USBTX, USBRX);
     
@@ -20,44 +22,102 @@
     double EMG_left_value;
     double EMG_left_f1;
     double EMG_left_f2;
+    double EMG_left_f3;
+    double EMG_left_f4;
+    double EMG_left_f5;
+    double EMG_left_f6;
     double EMG_left_abs;
     double EMG_right_value;
     double EMG_right_f1;
     double EMG_right_f2;
     double EMG_right_abs;
-    double Threshold1 = 0.08;
-    double Threshold2 = 0.06;    
-
+    double Threshold1 = 0.03;
+    double Threshold2 = 0.06;
+    
+    int N = 20;
+    double MAF_left[20];
+    double EMG_left_MAF;
+    
 // coëfficiënten
-    const double BiGainEMG_Lh = 0.723601, BiGainEMG_Ll=0.959332;
-    const double EMGh_a1 = -1.74355513773*BiGainEMG_Lh, EMGh_a2 = 0.80079826172*BiGainEMG_Lh, EMGh_b0 = 1.0*BiGainEMG_Lh, EMGh_b1 = -1.99697722433*BiGainEMG_Lh, EMGh_b2 = 1.0*BiGainEMG_Lh; //coefficients for high-pass filter
-    const double EMGl_a1 = 1.91721405106*BiGainEMG_Ll, EMGl_a2 = 0.92055427516*BiGainEMG_Ll, EMGl_b0 = 1.0*BiGainEMG_Ll, EMGl_b1 = 1.99999965990*BiGainEMG_Ll, EMGl_b2 = 1.0*BiGainEMG_Ll; // coefficients for low-pass filter
-       
+    const double BiGainEMG_H1 = 0.795375, BiGainEMG_H2 = 0.895763;
+    const double EMGH1_a1 = -1.56308931068*BiGainEMG_H1, EMGH1_a2 = 0.61765749583*BiGainEMG_H1, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99909075151*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter
+    const double EMGH2_a1 = -1.75651417587*BiGainEMG_H2, EMGH2_a2 = 0.82183182692*BiGainEMG_H2, EMGH2_b0 = 1.0*BiGainEMG_H2, EMGH2_b1 = -1.99470632157*BiGainEMG_H2, EMGH2_b2 = 1.0*BiGainEMG_H2; //coefficients for high-pass filter
+    
+    const double BiGainEMG_L1=0.959332, BiGainEMG_L2 = 0.223396;
+    const double EMGL1_a1 = -1.55576653052*BiGainEMG_L1, EMGL1_a2 = 0.61374320375*BiGainEMG_L1, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = -0.90928276835*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter
+    const double EMGL2_a1 = -1.79696141922*BiGainEMG_L2, EMGL2_a2 = 0.85096669383*BiGainEMG_L2, EMGL2_b0 = 1.0*BiGainEMG_L2, EMGL2_b1 = -1.75825311060*BiGainEMG_L2, EMGL2_b2 = 1.0*BiGainEMG_L2; // coefficients for low-pass filter
+
+    const double BiGainEMG_N1 = 1.0, BiGainEMG_N2 = 0.965081;
+    const double EMGN1_a1 = -1.56858163035*BiGainEMG_N1, EMGN1_a2 = 0.96424138362*BiGainEMG_N1, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61854514265*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for high-pass filter
+    const double EMGN2_a1 = -1.61100357722*BiGainEMG_N2, EMGN2_a2 = 0.96592170538*BiGainEMG_N2, EMGN2_b0 = 1.0*BiGainEMG_N2, EMGN2_b1 = -1.61854514265*BiGainEMG_N2, EMGN2_b2 = 1.0*BiGainEMG_N2; //coefficients for high-pass filter
+   
 // Filter creation    
-    biquadFilter EMG_highpass (EMGh_a1, EMGh_a2, EMGh_b0, EMGh_b1, EMGh_b2);        // creates the high pass filter
-    biquadFilter EMG_lowpass (EMGl_a1, EMGl_a2, EMGl_b0, EMGl_b1, EMGl_b2);         // creates the low pass filter
-    
+    biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2);        // creates the high pass filter
+    biquadFilter EMG_highpass2 (EMGH2_a1, EMGH2_a2, EMGH2_b0, EMGH2_b1, EMGH2_b2);
+    biquadFilter EMG_lowpass1 (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2);         // creates the low pass filter
+    biquadFilter EMG_lowpass2 (EMGL2_a1, EMGL2_a2, EMGL2_b0, EMGL2_b1, EMGL2_b2);  
+    biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2);           // creates the notch filter
+    biquadFilter EMG_notch2 (EMGN2_a1, EMGN2_a2, EMGN2_b0, EMGN2_b1, EMGN2_b2);
+        
 // EMG filtering function
 void EMGfilter() // Both EMG signals are filtered in one function and with the same filters
 {
     EMG_left_value = EMG_left.read();
-    EMG_left_f1 = EMG_highpass.step(EMG_left_value);
-    EMG_left_f2 = EMG_lowpass.step(EMG_left_f1);
+    EMG_left_f1 = EMG_highpass1.step(EMG_left_value);
+    EMG_left_f2 = EMG_highpass2.step(EMG_left_f1);
     EMG_left_abs = fabs(EMG_left_f2);
-    
-    EMG_right_value = EMG_right.read();
-    EMG_right_f1 = EMG_highpass.step(EMG_right_value);
-    EMG_right_f1 = EMG_lowpass.step(EMG_right_f1);
-    EMG_right_abs = fabs(EMG_right_f2);
+    EMG_left_f3 = EMG_lowpass1.step(EMG_left_abs);
+    EMG_left_f4 = EMG_lowpass2.step(EMG_left_f3);
+    EMG_left_f5 = EMG_notch1.step(EMG_left_f4);
+    EMG_left_f6 = EMG_notch1.step(EMG_left_f5);
+        
+//    EMG_right_value = EMG_right.read();
+//    EMG_right_f1 = EMG_highpass.step(EMG_right_value);
+//    EMG_right_f1 = EMG_lowpass.step(EMG_right_f1);
+//    EMG_right_abs = fabs(EMG_right_f2);
 }
 
+// Movingaverage Filter
+    void MovingAverageFilter()
+    {
+        int ArraySum;
+        for (int a = 0; a<N; a++)
+        {
+            ArraySum = ArraySum + MAF_left[a];
+        }
+        EMG_left_MAF = ArraySum/N;
+       // EMG_left_MAF = (MAF_left[0]+MAF_left[1]+MAF_left[2]+MAF_left[3]+MAF_left[5]+MAF_left[6]+MAF_left[7]+MAF_left[8]+MAF_left[9]+MAF_left[10]+MAF_left[11]+MAF_left[12]+MAF_left[13]+MAF_left[14]+MAF_left[15]+MAF_left[16]+MAF_left[17]+MAF_left[18]+MAF_left[19])/20;
+        MAF_left[19] = MAF_left[18];
+        MAF_left[18] = MAF_left[17];
+        MAF_left[17] = MAF_left[16];
+        MAF_left[16] = MAF_left[15];
+        MAF_left[15] = MAF_left[14];
+        MAF_left[14] = MAF_left[13];
+        MAF_left[13] = MAF_left[12];
+        MAF_left[12] = MAF_left[11];
+        MAF_left[11] = MAF_left[10];
+        MAF_left[10] = MAF_left[9];
+        MAF_left[9] = MAF_left[8];
+        MAF_left[8] = MAF_left[7];
+        MAF_left[7] = MAF_left[6];
+        MAF_left[6] = MAF_left[5];
+        MAF_left[5] = MAF_left[4];
+        MAF_left[4] = MAF_left[3];
+        MAF_left[3] = MAF_left[2];
+        MAF_left[2] = MAF_left[1];
+        MAF_left[1] = MAF_left[0];
+        MAF_left[0] = EMG_left_f6;
+    }
+    
 // HIDScope
     void ScopeSend()
     {
         scope.set(0, EMG_left_value);
-        scope.set(1, EMG_left_f1);
-        scope.set(2, EMG_left_f2);
-        scope.set(3, EMG_left_abs);
+        scope.set(1, EMG_left_f2);
+        scope.set(2, EMG_left_abs);
+        scope.set(3, EMG_left_f4);
+        scope.set(4, EMG_left_f6);
+        scope.set(5, EMG_left_MAF);
         scope.send();
     }
     
@@ -65,9 +125,10 @@
 {
     SampleEMG.attach(&EMGfilter, 0.002);
     ScopeTimer.attach(&ScopeSend, 0.002);
+    MovingAverage.attach(&MovingAverageFilter, 0.002);
     while(1) 
     {
-        if (EMG_left_abs > Threshold1)
+        if (EMG_left_f6 > Threshold1)
         {
             led.write(0);
         }