Diff: main.cpp

Revision:

4:bd4dd411dc7b

Parent:

3:b96d952083be

Child:

5:dd64e0cf20fe

--- a/main.cpp	Mon Oct 22 09:18:59 2018 +0000
+++ b/main.cpp	Mon Oct 22 13:05:16 2018 +0000
@@ -12,68 +12,112 @@
 
 
 // Define the HIDScope and Ticker objects
-HIDScope    scope(2);
+HIDScope    scope(4);
 Ticker      scopeTimer;
 Ticker      EMGTicker;
 
 // BiQuad filter
-BiQuad HP_emg(1,-2,1,1.142980502539901,0.412801598096189); //Highpassfilter
+BiQuad HP_emg1(1,-2,1,1.142980502539901,0.412801598096189); //Highpassfilter
+BiQuad HP_emg2(1,-2,1,1.142980502539901,0.412801598096189); //Highpassfilter
 
-double EMGdata;
+// Global variables
+double EMGdata1;
+double EMGdata2;
+int    count;
 
 void ReadEMG()
 { 
-    EMGdata = a0.read(); // store values in the scope
+    EMGdata1 = a0.read(); // store values in the scope
+    EMGdata2 = a1.read();
+    
 }
 
-double EMG_HP(double EMGdata)
+double EMG_HP1(double EMGdata) 
 {
-    double HP_abs_EMGdata = HP_emg.step(EMGdata);
+    double HP_abs_EMGdata = HP_emg1.step(EMGdata);
+    
+    return fabs(HP_abs_EMGdata);
+}
+
+double EMG_HP2(double EMGdata)
+{
+    double HP_abs_EMGdata = HP_emg2.step(EMGdata);
     
     return fabs(HP_abs_EMGdata);
 }
 
-double Moving_mean(double HP_abs_EMGdata)
+// Moving mean of EMG data 1
+double Moving_mean1(double HP_abs_EMGdata1)
 {
-    int P = 50;
-    double EMGarray[P];
+    int P1 = 50;
+    static double EMGarray1[50];
     
-    for(int i = P-1; i >= 0; i--)
+    for(int i1 = P1-1; i1 >= 1; i1--)
     {
-        if (i == 0) 
-        {
-            EMGarray[i] = HP_abs_EMGdata;
-        }
-        else 
-        {
-        EMGarray[i] = EMGarray[i-1];
-        }   
+        EMGarray1[i1] = EMGarray1[i1-1];  
     }
+    EMGarray1[0] = HP_abs_EMGdata1;
     
-    double sum;
+    double sum1 = 0.0;
     
-    for(int j=0; j<50; j++)
+    for(int j1=0; j1<50; j1++)
     {
-        sum += EMGarray[j];
+        sum1 += EMGarray1[j1];
         
     }
     
-    pc.printf("sum = %lf", sum);
-    double EMG_filt = sum / (double) P;
+    double EMG_filt_1 = sum1 / (double) P1;
+    
+    return EMG_filt_1;
+}
+
+// Moving mean of EMG data 2
+double Moving_mean2(double HP_abs_EMGdata2)
+{
+    int P2 = 50;
+    static double EMGarray2[50];
     
-    return(EMG_filt);
+    for(int i2 = P2-1; i2 >= 1; i2--)
+    {
+        EMGarray2[i2] = EMGarray2[i2-1];    
+    }
+    EMGarray2[0] = HP_abs_EMGdata2;
+    
+    double sum2;
+    
+    for(int j2=0; j2<50; j2++)
+    {
+        sum2 += EMGarray2[j2];
+        
+    }
+    
+    double EMG_filt_2 = sum2 / (double) P2;
+    
+    return(EMG_filt_2);
 }
 
 void EMG_filtering ()
 {      
     ReadEMG();
-    scope.set(0, EMGdata);
+    scope.set(0, EMGdata1);
+    scope.set(1, EMGdata2);
+    
+    double HP_abs_EMGdata1 =  EMG_HP1(EMGdata1);
+    double HP_abs_EMGdata2 =  EMG_HP2(EMGdata2);
+    
+    double EMG_filt1 = Moving_mean1(HP_abs_EMGdata1);
+    double EMG_filt2 = Moving_mean2(HP_abs_EMGdata2);
     
-    volatile double HP_abs_EMGdata =  EMG_HP(EMGdata);
-    volatile double EMG_filt = Moving_mean(HP_abs_EMGdata);
+    scope.set(2, EMG_filt1);
+    scope.set(3, EMG_filt2);
+    
+    count++;
     
-    scope.set(1, EMG_filt);
-    pc.printf("HP_abs_EMGdata = %lf \t EMG filt= %lf \n\r", HP_abs_EMGdata, EMG_filt);
+    if (count == 50)
+    {
+        pc.printf("EMG filt 1 = %lf \t EMG filt 2 = %lf \n\r", EMG_filt1, EMG_filt2);
+        count = 0;
+    }
 }
  
 int main()