File content as of revision 12:12c162dc8893:

#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(3);

//Notch filter
BiQuadChain notch_50_0;
BiQuadChain notch_50_1;
BiQuadChain notch_50_2;
BiQuad bq03( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621);
BiQuad bq04( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725);
BiQuad bq05( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072);
BiQuad bq13( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621);
BiQuad bq14( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725);
BiQuad bq15( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072);
BiQuad bq23( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621);
BiQuad bq24( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725);
BiQuad bq25( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072);

//High pass filter
BiQuadChain high_pass_0;
BiQuadChain high_pass_1;
BiQuadChain high_pass_2;
BiQuad bq06( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630);
BiQuad bq07( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694);
BiQuad bq16( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630);
BiQuad bq17( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694);
BiQuad bq26( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630);
BiQuad bq27( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694);
//BiQuad bq8( 0.92490714701,-1.84981429401, 0.92490714701, -1.90032503529, 0.9352152620);

//Low pass filter
BiQuadChain low_pass_0;
BiQuadChain low_pass_1;
BiQuadChain low_pass_2;
BiQuad bq9( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240);
BiQuad bq10( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411);
BiQuad bq11( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909);
BiQuad bq19( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240);
BiQuad bq110( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411);
BiQuad bq111( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909);
BiQuad bq29( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240);
BiQuad bq210( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411);
BiQuad bq211( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909);

//Ticker
Ticker emgSampleTicker;

//Timeout to change state after 5 seconds
Timeout change_state;

//Timeout to change state after 15 seconds
Timeout change_state2;

//LED
DigitalOut led(LED_RED);

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

//Go flag for emg sample
bool go_emgSample;

//Variables for intermediate filter values
double emg_sample[3];
double emg_low_passed_200[3];
double emg_notch[3];
double emg_high_passed[3];
double emg_low_passed[3];
double min_emg[3];
double max_emg[3];

//Calculating normalized outputs
double Norm_EMG_0;
double Norm_EMG_1;
double Norm_EMG_2;

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

void emgSample() {
    go_emgSample = true;
}

void calibrate() {
    state = STATE_CALIBRATION;  
    led.write(0);
}

void run() {
    state = STATE_RUN;  
    led.write(1);
}

void 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();
            //pc.printf("%f - %f - %f \r\n", emg_sample[0], emg_sample[1], emg_sample[2]); 
            //filter out the 50Hz components with a notch filter
            emg_notch[0] = notch_50_0.step(emg_sample[0]);
            emg_notch[1] = notch_50_1.step(emg_sample[1]);
            emg_notch[2] = notch_50_2.step(emg_sample[2]);
            //high pass the signal (removing motion artifacts and offset)
            emg_high_passed[0] = high_pass_0.step(emg_notch[0]);
            emg_high_passed[1] = high_pass_1.step(emg_notch[1]);
            emg_high_passed[2] = high_pass_2.step(emg_notch[2]);    
            float emg_fabs[3];
            emg_fabs[0] = fabs(emg_high_passed[0]);
            emg_fabs[1] = fabs(emg_high_passed[1]);
            emg_fabs[2] = fabs(emg_high_passed[2]);
            //low pass the rectified emg signal
            emg_low_passed[0] = low_pass_0.step(emg_fabs[0]);
            emg_low_passed[1] = low_pass_1.step(emg_fabs[1]);
            emg_low_passed[2] = low_pass_2.step(emg_fabs[2]);
            
            //pc.printf("%f - %f - %f \r\n", emg_low_passed[0], emg_low_passed[1], emg_low_passed[2]);
            //Calculating min value and max value of emg signal
            if(state == STATE_CALIBRATION)
            {
                if (start_calibration == 0) {
                    min_emg[0] = emg_low_passed[0];
                    max_emg[0] = emg_low_passed[0];
                    min_emg[1] = emg_low_passed[1];
                    max_emg[1] = emg_low_passed[1];
                    min_emg[2] = emg_low_passed[2];
                    max_emg[2] = emg_low_passed[2];
                    start_calibration++;
                }
                else {
                    //finding min and max of emg0
                    
                    if (emg_low_passed[0] < min_emg[0]) {
                        min_emg[0] = emg_low_passed[0];
                    }
                    else if (emg_low_passed[0] > max_emg[0]) {
                        max_emg[0] = emg_low_passed[0];
                    }
                    
                    //finding min and max of emg1
                    if (emg_low_passed[1] < min_emg[1]) {
                        min_emg[1] = emg_low_passed[1];
                    }
                    else if (emg_low_passed[1] > max_emg[1]) {
                        max_emg[1] = emg_low_passed[1];
                    }
                    
                    //finding min and max of emg2
                    if (emg_low_passed[2] < min_emg[2]) {
                        min_emg[2] = emg_low_passed[2];
                    }
                    else if (emg_low_passed[2] > max_emg[2]) {
                        max_emg[2] = emg_low_passed[2];
                    }
                }   
            } 
            
            //calculating input_forces for controller
            Norm_EMG_0 = (emg_low_passed[0] - min_emg[0])/(max_emg[0]-min_emg[0]);
            Norm_EMG_1 = (emg_low_passed[1] - min_emg[1])/(max_emg[1]-min_emg[1]);
            Norm_EMG_2 = (emg_low_passed[2] - min_emg[2])/(max_emg[2]-min_emg[2]);
            
            //Send scope data
            //scope.set(0,Norm_EMG_0);
            //scope.set(1,Norm_EMG_1);
            //scope.set(2,Norm_EMG_2);

            go_emgSample = false;
        }   
    }