File content as of revision 5:86f0b27c58ea:

#include "mbed.h"
#include "math.h"
#include "HIDScope.h"

// CODE THE PART WHERE WE PUT THE SIGNAL INTO RAW_SIGNAL
// IF NEEDED WE CAN HAVE THE FILTERS OF SECOND ORDER

// 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 );

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[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[2]; // raw_emg_1 filtered by high pass filter
double filt1[2];
//double filt2[2];
//double filt3[2];

double filt_filt0[2]; // filt1 filtered by low pass filter
double filt_filt1[2];
//double filt_filt2[2];
//double filt_filt3[2];

double emgA;  // not an array
//double emgB;

double emg_absA[2]; // absolute value applied to emg
//double emg_absB[2];

double emg_filtA[2]; 
//double emg_filtB[2];

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;

bool speedA= false; // running motor at high speed
//bool speedB= false;

double T=0.002; // time between two samples
 
 
void sample()
{
    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();

    scope.set(0, fabs(emg0.read()- emg1.read() );
    scope.send();
}

 
// BIG FUNCTION THAT INCLUDES EVERYTHING

void processing_signals()
{
     
    // LOWPASS FILTER
    double wclp=300*2*pi;  // cutoff omega for low pass filter
    
    // initialization of the filt arrays
    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);
    
    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]=filt_filt0[1];
    filt_filt1[0]=filt_filt1[1];
    //filt_filt2[0]=filt_filt2[1];
    //filt_filt3[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);
    
    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
        
    emgA=filt_filt0[1]-filt_filt1[1];
    emg_absA[0]=emg_absA[1];
    emg_absA[1]=fabs(emgA);
    
    //emgB=filt_filt2[1]-filt_filt3[1];
    //emg_absB[0]=emg_absB[1];
    //emg_absB[1]=fabs(emgB);
    
    
    // APPLYING ANOTHER LOW PASS FILTER WITH SUPER LOW CUTOFF FREQUENCY
    
    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;  
    
    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];
    
    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];
    
    scope.set(1, emg_filtA[1] );
    scope.send();
    
    
int main()
{
    sampling.attach(&sample, 0.002);
    processing.attach(&processing_signals, 0.002);
    
    while (true)
    {
        
    }
    return 0;
}