Diff: main.cpp

Revision:

2:c177a3e9708e

Parent:

1:0b3280b1795e

Child:

3:15eeab5ba885

--- a/main.cpp	Mon Oct 14 16:43:28 2019 +0000
+++ b/main.cpp	Mon Oct 14 17:30:57 2019 +0000
@@ -2,6 +2,7 @@
 #include "math.h"
 
 // CODE THE PART WHERE WE PUT THE SIGNAL INTO RAW_SIGNAL
+// IF NEEDED WE CAN HAVE THE FILTERS OF SECOND ORDER
 
 // Defining objects
 
@@ -19,36 +20,32 @@
 Ticker sampling; // ticker for the sampling of the signal, every 0.002s a value is sampled
 Ticker processing; //ticker that defines the signal processed, every second a signal is defined
 
-double raw_signal_0[500];  // has the values measured via the electrodes
-double raw_signal_1[500];
-double raw_signal_2[500];
-double raw_signal_3[500];
+double raw_signal_0[2];  // has the values measured via the electrodes
+double raw_signal_1[2];
+double raw_signal_2[2];
+double raw_signal_3[2];
 
-double filt0[500]; // raw_emg_1 filtered by high pass filter
-double filt1[500];
-double filt2[500];
-double filt3[500];
-
-double filt_filt0[500]; // filt1 filtered by low pass filter
-double filt_filt1[500];
-double filt_filt2[500];
-double filt_filt3[500];
+double filt0[2]; // raw_emg_1 filtered by high pass filter
+double filt1[2];
+double filt2[2];
+double filt3[2];
 
-double emgA[500]; //difference between the two channels
-double emgB[500];
+double filt_filt0[2]; // filt1 filtered by low pass filter
+double filt_filt1[2];
+double filt_filt2[2];
+double filt_filt3[2];
 
-
-double emg_absA[500]; // absolute value applied to emg
-double emg_absB[500];
+double emgA;  // not an array
+double emgB;
 
-double emg_maA[500]; // moving average applied to emg_abs
-double emg_maB[500];
+double emg_absA[2]; // absolute value applied to emg
+double emg_absB[2];
 
-double emg_rmsA[500]; // rms applied to emg_ma
-double emg_rmsB[500];
+double emg_filtA[2]; 
+double emg_filtB[2];
 
-double emg_normA[500]; // normalization of emg_rms
-double emg_normB[500];
+double emg_normA[2]; // normalization of emg_rms
+double emg_normB[2];
 
 bool modeA= false; // running motor at intermediate speed, derived from threshold
 bool modeB= false;
@@ -61,11 +58,16 @@
  
 void sample()
 {
-    raw_signal_0[mark]=emg0.read();
-    raw_signal_1[mark]=emg1.read();
-    raw_signal_2[mark]=emg2.read();
-    raw_signal_3[mark]=emg3.read();
-    mark++;
+    // raw_signal=[ value(T-2), value(T-1), value(T)]
+    raw_signal_0[0]=raw_signal_0[1];
+    raw_signal_1[0]=raw_signal_1[1];
+    raw_signal_2[0]=raw_signal_2[1];
+    raw_signal_3[0]=raw_signal_3[1];
+     
+    raw_signal_0[1]=emg0.read();
+    raw_signal_1[1]=emg1.read();
+    raw_signal_2[1]=emg2.read();
+    raw_signal_3[1]=emg3.read();
 }
 
  
@@ -73,190 +75,81 @@
 
 void processing_signals()
 {
-    mark=0;
-    
+     
     // LOWPASS FILTER
     double wclp=300*2*pi;  // cutoff omega for low pass filter
     
     // initialization of the filt arrays
-    filt0[0]=(raw_signal_0[0]+raw_signal_0[1])/2; // we take the mean of the two first values to initialize the signal
-    filt1[0]=(raw_signal_1[0]+raw_signal_1[1])/2;  
-    filt2[0]=(raw_signal_2[0]+raw_signal_2[1])/2;
-    filt3[0]=(raw_signal_3[0]+raw_signal_3[1])/2; 
+    filt0[0]=filt0[1];
+    filt1[0]=filt1[1];
+    filt2[0]=filt2[1];
+    filt3[0]=filt3[1];
+ 
     
     // initialization of the parameters
     double b0=(wclp*T)/(wclp*T+2);
     double b1=(wclp*T)/(wclp*T+2);
     double a1=(wclp*T-2)/(wclp*T+2);
-    int a;
-    for (a=1;a<500;a++) // the for loop starts at i=1 because we have already set values for i=0
-    {
-        filt0[a]=b0*raw_signal_0[a]+b1*raw_signal_0[a-1]-a1*filt0[a-1];
-        filt1[a]=b0*raw_signal_1[a]+b1*raw_signal_1[a-1]-a1*filt1[a-1];
-        filt2[a]=b0*raw_signal_2[a]+b1*raw_signal_2[a-1]-a1*filt2[a-1];
-        filt3[a]=b0*raw_signal_3[a]+b1*raw_signal_3[a-1]-a1*filt3[a-1];
-    }
+    
+    filt0[1]=b0*raw_signal_0[1]+b1*raw_signal_0[0]-a1*filt0[0];
+    filt1[1]=b0*raw_signal_1[1]+b1*raw_signal_1[0]-a1*filt1[0];
+    filt2[1]=b0*raw_signal_2[1]+b1*raw_signal_2[0]-a1*filt2[0];
+    filt3[1]=b0*raw_signal_3[1]+b1*raw_signal_3[0]-a1*filt3[0];
+    
     
     // HIGH PASS FILTER
     double wchp=10*2*pi;
     
     // initialization by the mean of the two first values
-    filt_filt0[0]=(filt0[0]+filt0[1])/2;
-    filt_filt1[0]=(filt1[0]+filt1[1])/2;
-    filt_filt2[0]=(filt2[0]+filt2[1])/2;
-    filt_filt4[0]=(filt4[0]+filt4[1])/2;
+    filt_filt0[0]=filt_filt0[1];
+    filt_filt1[0]=filt_filt1[1];
+    filt_filt2[0]=filt_filt2[1];
+    filt_filt4[0]=filt_filt3[1];
     
     // initialization of the parameters
     double d0=2/(wchp*T+2);
     double d1=2/(wchp*T+2);
     double c1=(wchp*T-2)/(wchp*T+2);
-    int b;
-    for (b=1;b<500;b++)
-    {
-        filt_filt0[b]=d0*filt0[b]+d1*filt0[b-1]-c1*filt_filt0[b-1];
-        filt_filt1[b]=d0*filt1[b]+d1*filt1[b-1]-c1*filt_filt1[b-1];
-        filt_filt2[b]=d0*filt2[b]+d1*filt2[b-1]-c1*filt_filt2[b-1];
-        filt_filt3[b]=d0*filt3[b]+d1*filt3[b-1]-c1*filt_filt3[b-1];
-    } 
+    
+    filt_filt0[1]=d0*filt0[1]+d1*filt0[0]-c1*filt_filt0[0];
+    filt_filt1[1]=d0*filt1[1]+d1*filt1[0]-c1*filt_filt1[0];
+    filt_filt2[1]=d0*filt2[1]+d1*filt2[0]-c1*filt_filt2[0];
+    filt_filt3[1]=d0*filt3[1]+d1*filt3[0]-c1*filt_filt3[0];
+     
     
     // DIFFERENCE OF THE SIGNALS AND ABSOLUTE VALUE
-    int c;
-    for (c=0;c<500;c++)
-    {
-        emgA[c]=filt_filt0[c]-filt_filt1[c];
-        emg_absA[c]=fabs(emgA[c]);
-        emgB[c]=filt_filt2[c]-filt_filt3[c];
-        emg_absB[c]=fabs(emgB[c]);
-    }
+        
+    emgA=filt_filt0[1]-filt_filt1[1];
+    emg_absA[0]=emg_absA[1];
+    emg_absA[1]=fabs(emgA);
     
-    // MOVING AVERAGE
-    int i;
-    for(i=5;i<500;i++)
-    {
-        double val4=(emg_absA[i-4]+emg_absA[i-3]+emg_absA[i-2]+emg_absA[i-1]+emg_absA[i])/5; 
-        double val3=(emg_absA[i-3]+emg_absA[i-2]+emg_absA[i-1]+emg_absA[i])/4;
-        double val2=(emg_absA[i-2]+emg_absA[i-1]+emg_absA[i])/3;
-        double val1=(emg_absA[i-1]+emg_absA[i])/2;
-        double val0=emg_absA[i];
-        emg_maA=(val0+val1+val2+val3+val4)/5;
-    }
-    int ii;
-    for(ii=5;ii<500;ii++) // we can reuse val4,3,2,1,0 because they are local variables
-    {
-        double val4=(emg_absB[ii-4]+emg_absB[ii-3]+emg_absB[ii-2]+emg_absB[ii-1]+emg_absB[ii])/5; 
-        double val3=(emg_absB[ii-3]+emg_absB[ii-2]+emg_absB[ii-1]+emg_absB[ii])/4;
-        double val2=(emg_absB[ii-2]+emg_absB[ii-1]+emg_absB[ii])/3;
-        double val1=(emg_absB[ii-1]+emg_absB[ii])/2;
-        double val0=emg_absB[ii];
-        emg_maB=(val0+val1+val2+val3+val4)/5;
-    }
-    
-    // ROOT MEAN SQUARE
-    
-    int d;
-    double sumA, sumB;
-    for (d=5;d<500;d++)
-    {
-        sumA=emg_maA[d-4]^2+emg_maA[d-3]^2+emg_maA[d-2]^2+emg_maA[d-1]^2+emg_maA[d]^2;
-        emg_rmsA[d]=sqrt(sumA/5);
-        sumB=emg_maB[d-4]^2+emg_maB[d-3]^2+emg_maB[d-2]^2+emg_maB[d-1]^2+emg_maB[d]^2;
-        emg_rmsB[d]=sqrt(sumB/5);
-    }
+    emgB=filt_filt2[1]-filt_filt3[1];
+    emg_absB[0]=emg_absB[1];
+    emg_absB[1]=fabs(emgB);
     
     
-    emg_rmsA[0]=emg_absA[0];
-    emg_rmsA[1]=emg_absA[1];
-    emg_rmsA[2]=emg_absA[2];
-    emg_rmsA[3]=emg_absA[3];
-    emg_rmsA[4]=emg_absA[4];
-    
-    emg_rmsB[0]=emg_absB[0];
-    emg_rmsB[1]=emg_absB[1];
-    emg_rmsB[2]=emg_absB[2];
-    emg_rmsB[3]=emg_absB[3];
-    emg_rmsB[4]=emg_absB[4];
-    
-    // NORMALIZATION
+    // APPLYING ANOTHER LOW PASS FILTER WITH SUPER LOW CUTOFF FREQUENCY
     
-    int e; // computes the maximum and the minimum of the array
-    double MEANA=(emg_rmsA[250]+emg_rmsA[251])/2; // random points mean
-    double MAXA=MEANA+1;
-    double MINA=MEANA-1;
-    for (e=0; e<500;e++)
-    {
-        if (emg_rmsA[e]>MAXA)
-        {
-            MAXA=emg_rmsA[e];
-        }
-        if (emg_rmsA[e]<MINA)
-        {
-            MINA=emg_rmsA[e];
-        }
-    }
+    double wcllp=2*pi; // low low pass cutoff frequency
+    
+    emg_filtA[0]=(emg_absA[0]+emg_absA[1])/2; // we take the mean of the two first values to initialize the signal
+    emg_filtB[0]=(emg_absB[0]+emg_absB[1])/2;  
     
-    int f;
-    double MEANB=(emg_rmsB[250]+emg_rmsB[251])/2; // random points mean
-    double MAXB=MEANB+1;
-    double MINB=MEANB-1;
-    for (f=0; f<500; f++)
-    {
-        if (emg_rmsB[f]>MAXB)
-        {
-            MAXB=emg_rmsB[f];
-        }
-        if (emg_rmsB[e]<MINB)
-        {
-            MINB=emg_rmsB[f];
-        }
-    }
-    
-    int j;
-    for (j=0;j<500;j++)
-    {
-        emg_normA[j]=(emg_rmsA[j]-MINA)/(MAXA-MINA);
-        emg_normB[j]=(emg_rmsB[j]-MINB)/(MAXB-MINB);
-    }
+    double k0=(wcllp*T)/(wcllp*T+2);
+    double k1=(wcllp*T)/(wcllp*T+2);
+    double l1=(wcllp*T-2)/(wcllp*T+2);
+     
+    emg_filtA[0]=emg_filtA[1];
+    emg_filtB[0]=emg_filtB[1];
     
-    // compute some parameter on the array (average) to set the boolean to true or false by thresholding  => parameter to be determined by matlab
-    double meanA, meanB;
-    double sumA, sumB;
-    int p;
-    for (p=0;p<500;p++)
-    {
-        sumA=sumA+emg_normA[p];
-        sumB=sumB+emg_normB[p];
-    }
-    meanA=sumA/500;
-    meanB=sumB/500;
-    
-    double threshold=0.5; // threshold for setting the motor yes or no
-    double threshold_speed= 0.7; // threshold for if we go full speed or no
-    // THESE VALUES CAN BE CHANGED IF NEEDED
+    emg_filtA[1]=k0*emg_absA[1]+k1*emg_absA[0]-l1*emg_filtA[0];
+    emg_filtB[1]=k0*emg_absB[1]+k1*emg_absB[0]-l1*emg_filtB[0];
     
-    if (meanA>threshold)
-    {
-        modeA=true;
-    }
-    
-    if (meanB>threshold)
-    {
-        modeB=true;
-    }
-    if (meanA>threshold_speed)
-    {
-        speedA=true;
-    }
-    
-    if (meanB>threshold_speed)
-    {
-        speedB=true;
-    }
-}
     
 int main()
 {
     sampling.attach(&sample, 0.002);
-    processing.attach(&processing_signals, 1.0);
+    processing.attach(&processing_signals, 0.002);
     
     while (true)
     {