Diff: main.cpp

Revision:

0:971166224b2d

Child:

1:0b3280b1795e

diff -r 000000000000 -r 971166224b2d main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 14 09:34:17 2019 +0000
@@ -0,0 +1,266 @@
+#include "mbed.h"
+#include "math.h"
+
+// CODE THE PART WHERE WE PUT THE SIGNAL INTO RAW_SIGNAL
+
+// Defining objects
+
+AnalogIn emg0( A0 ); // for movement up/down, named A
+AnalogIn emg1( A1 );
+
+AnalogIn emg2( A2 ); // for movement forward/backward, named B
+AnalogIn emg3( A3 );
+
+int mark=0;  // allows us to know where we are exactly in the sampling process
+
+const double pi=3.14;
+
+// define two functions that will be attached to the tickers
+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 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 emgA[500]; //difference between the two channels
+double emgB[500];
+
+
+double emg_absA[500]; // absolute value applied to emg
+double emg_absB[500];
+
+double emg_maA[500]; // moving average applied to emg_abs
+double emg_maB[500];
+
+double emg_rmsA[500]; // rms applied to emg_ma
+double emg_rmsB[500];
+
+double emg_normA[500]; // normalization of emg_rms
+double emg_normB[500];
+
+bool run_mot_1= false; // running motor at intermediate speed, derived from threshold
+bool run_mot_2= false;
+
+bool speed1= false; // running motor at high speed
+bool speed2= false;
+
+double T=0.002; // time between two samples
+ 
+ 
+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++;
+}
+
+ 
+// BIG FUNCTION THAT INCLUDES EVERYTHING
+
+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; 
+    
+    // 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];
+    }
+    
+    // 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;
+    
+    // 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];
+    } 
+    
+    // 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]);
+    }
+    
+    // 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);
+    }
+    
+    
+    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
+    
+    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];
+        }
+    }
+    
+    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);
+    }
+    
+    // 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
+    
+    if (meanA>threshold)
+    {
+        run_mot_1=true;
+    }
+    
+    if (meanB>threshold)
+    {
+        run_mot_2=true;
+    }
+    if (meanA>threshold_speed)
+    {
+        speed1=true;
+    }
+    
+    if (meanB>threshold_speed)
+    {
+        run_mot_2=true;
+    }
+}
+    
+int main()
+{
+    sampling.attach(&sample, 0.002);
+    processing.attach(&processing_signals, 1.0);
+    
+    while (true)
+    {
+        
+    }
+    return 0;
+}
\ No newline at end of file