File content as of revision 1:0b3280b1795e:

#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 modeA= false; // running motor at intermediate speed, derived from threshold
bool modeB= false;

bool speedA= false; // running motor at high speed
bool speedB= 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)
    {
        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);
    
    while (true)
    {
        
    }
    return 0;
}