File content as of revision 2:13fa37643b8a:

#include "mbed.h"
#include "BiQuad.h"
#include "HIDScope.h"
//Enum with states
enum states {STATE_DEFAULT , STATE_CALIBRATION, STATE_RUN};

//Variable called 'state'
states state = STATE_DEFAULT;

//Creating two scope channels
HIDScope scope(2);

//Notch filter
BiQuadChain notch_50;
BiQuad bq1( 1.00000000000, -1.60956348896,  1.00000000000, -1.40195621505,  0.74203282402);
BiQuad bq2( 1.00000000000, -1.60724786352,  1.00000000000, -1.33646101015,  0.85967899264);
BiQuad bq3( 1.00000000000, -1.61186693071,  1.00000000000, -1.64415455961,  0.89726621230);

//High pass filter
BiQuadChain high_pass;
BiQuad bq4( 1.00000000000, -1.99999967822,  1.00000000000, -1.98388291862,  0.98395921205);
BiQuad bq5( 1.00000000000, -1.99999812453,  1.00000000000, -1.99324612474,  0.99332432675);

//Ticker
Ticker emgSampleTicker;

//LED
DigitalOut led(LED_RED);

//Button
InterruptIn button(SW2);

//Time led blink
Timeout blink_timer;

//Emg input
AnalogIn emg0( A0 );
AnalogIn emg1( A1 );
AnalogIn emg2( A2 );

bool go_emgSample;
bool go_find_minmax;
double emg_sample[3];
double emg_notch[3];
double emg_high_passed[3];
double emg_rectified;
double min_emg[3];
double max_emg[3];

const int n = 200;
int counter = 0;
double RMSArray0[n] = {0};
double RMSArray1[n] = {0};
double RMSArray2[n] = {0};
double RMS0;
double RMS1;
double RMS2;
double SumRMS0;
double SumRMS1;
double SumRMS2;

double input_force0;
double input_force1;
double input_force2;

//count for emg min max
int start_calibration = 0;

void emgSample() {
    go_emgSample = true;
}

void blink();

void calibrate();

void EMG_filter();

int main() {
    //combine biquads in biquad chains for notch/high- low-pass filters
    notch_50.add( &bq1 ).add( &bq2 ).add( &bq3 );
    high_pass.add( &bq4 ).add( &bq5 );
    button.mode(PullUp);
    button.rise(&calibrate);
    led.write(1);
    emgSampleTicker.attach( &emgSample, 0.002);
    while( true ){
        if(go_emgSample == true){
            EMG_filter();
        }
   }     
}


void EMG_filter() {
    if(go_emgSample == true){
            //read the emg signal
            emg_sample[0] = emg0.read();
            emg_sample[1] = emg1.read();
            emg_sample[2] = emg2.read();
            
            for (int i = 0; i < 3; i++){
                //filter out the 50Hz components with a notch filter
                //emg_notch[i] = notch_50.step(emg_sample[i]);
            
                //high pass the signal (removing motion artifacts and offset)
                emg_high_passed[i] = high_pass.step(emg_sample[i]);
            }
            
            //Calculating RMS
            SumRMS0 -= pow(RMSArray0[counter],2);
            SumRMS1 -= pow(RMSArray1[counter],2);
            SumRMS2 -= pow(RMSArray2[counter],2);
            
            RMSArray0[counter] = emg_high_passed[0];
            RMSArray1[counter] = emg_high_passed[1];
            RMSArray2[counter] = emg_high_passed[2];
            
            SumRMS0 += pow(RMSArray0[counter],2);
            SumRMS1 += pow(RMSArray1[counter],2);
            SumRMS2 += pow(RMSArray2[counter],2);
            
            counter++;
            if (counter == n){
                counter = 0;
            }
            
            RMS0 = sqrt(SumRMS0/n);
            RMS1 = sqrt(SumRMS1/n);
            RMS2 = sqrt(SumRMS2/n);
            
            //Calculating min value and max value of emg signal
            //if(state == STATE_CALIBRATION)
            //{
            //    if (start_calibration == 0) {
            //        min_emg[0] = RMS0;
            //        max_emg[0] = RMS0;
            //        min_emg[1] = RMS1;
            //        max_emg[1] = RMS1;
            //        min_emg[2] = RMS2;
            //        max_emg[2] = RMS2;
            //        start_calibration++;
            //    }
            //    else {
            //        //finding min and max of emg0
            //        if (RMS0 < min_emg[0]) {
            //            min_emg[0] = RMS0;
            //        }
            //        else if (RMS0 > max_emg[0]) {
            //            max_emg[0] = RMS0;
            //        }
            //        
            //        //finding min and max of emg1
            //        if (RMS1 < min_emg[1]) {
            //            min_emg[1] = RMS1;
            //        }
            //        else if (RMS1 > max_emg[1]) {
            //            max_emg[1] = RMS1;
            //        }
            //        
            //        //finding min and max of emg2
            //        if (RMS2 < min_emg[2]) {
            //            min_emg[2] = RMS2;
            //        }
            //        else if (RMS2 > max_emg[2]) {
            //            max_emg[2] = RMS2;
            //        }
            //    }   
            //} 
            
            //calculating input_forces for controller
            input_force0 = (RMS0 - min_emg[0])/(max_emg[0]-min_emg[0]);
            input_force1 = (RMS1 - min_emg[1])/(max_emg[1]-min_emg[1]);
            input_force2 = (RMS2 - min_emg[2])/(max_emg[2]-min_emg[2]);
            
            //Send scope data
            scope.set(0,emg_sample[0]);
            scope.set(1,input_force0);
            //scope.set(2,input_force1);
            //scope.set(3,input_force2);
            scope.send();

            go_emgSample = false;
        }   
    }

void calibrate() {
    state = STATE_CALIBRATION;  
    switch(start_calibration) {
        case 0 :
            break;
        case 1 :
            min_emg[0] = RMS0;
            break;
        case 2 :
            max_emg[0] = RMS0;
            break;
        case 3 :
            min_emg[1] = RMS1;
            break;
        case 4:
            max_emg[1] = RMS1;
            break;
        case 5:
            min_emg[2] = RMS2;
            break;
        case 6:
            max_emg[2] = RMS2;
            break;
    }
    if (start_calibration < 7){
        led.write(0);
        blink_timer.attach(&blink,0.5);
        start_calibration++;
    }
}

void blink() {
    led.write(1);
}