File content as of revision 30:2c67abcdb892:

#include "mbed.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "QEI.h"

//Define objects
    AnalogIn    emg1_in( A0 ); /* read out the signal */
    AnalogIn    emg2_in( A1 );
    AnalogIn    emg3_in( A2 );
    AnalogIn    emg4_in( A3 );
    DigitalIn   max_reader12( SW2 ); /* define button press */
    DigitalIn   max_reader34( SW3 );

    Ticker      main_timer;
    Ticker      max_read1;
    Ticker      max_read3;
    HIDScope    scope( 4 );
    DigitalOut  red(LED_RED);
    DigitalOut  blue(LED_BLUE);
    DigitalOut  green(LED_GREEN);
    MODSERIAL   pc(USBTX, USBRX);


// EMG variables
    //Right Biceps


    double emg1;
    double emg1highfilter;
    double emg1notchfilter;
    double emg1abs;
    double emg1lowfilter;
    double emg1peak;
    double max1;
    double maxpart1;
    // Left Biceps
    double emg2;
    double emg2highfilter;
    double emg2notchfilter;
    double emg2abs;
    double emg2lowfilter;
    double emg2peak;
    double max2;
    double maxpart2;
    // Left Lower Arm
    double emg3;
    double emg3highfilter;
    double emg3notchfilter;
    double emg3abs;
    double emg3lowfilter;
    double emg3peak;
    double max3;
    double maxpart3;
    // Right Lower Arm
    double emg4;
    double emg4highfilter;
    double emg4notchfilter;
    double emg4abs;
    double emg4lowfilter;
    double emg4peak;
    double max4;
    double maxpart4;
   
// BiQuad Filter Settings
    // Right Biceps
    BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); /* Filter at 10 Hz */
    BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); /* Filter at 50 Hz */
    BiQuad filterpeak1(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01); /* 4dB Gain peak at 11 Hz */
    BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); /* Filter at 15 Hz */
    // Left Biceps
    BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
    BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
    BiQuad filterpeak2(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
    BiQuad filterlow2(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
    // Left Lower Arm OR Triceps
    BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
    BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
    BiQuad filterpeak3(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
    BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
    // Right Lower Arm OR Triceps
    BiQuad filterhigh4(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
    BiQuad filternotch4(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
    BiQuad filterpeak4(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
    BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
    //

// Finding max values for correct motor switch if the button is pressed
void get_max1(){
    if (max_reader12==0){
            green = !green;
            red = 1;
            blue = 1;
            for(int n=0;n<2000;n++){  /* measure 2000 samples and filter it */

            emg1 = emg1_in.read();      /* read out emg */
            emg1highfilter = filterhigh1.step(emg1); /* high pass filtered */
            emg1notchfilter = filternotch1.step(emg1highfilter); /* notch filtered */
            emg1abs = fabs(emg1notchfilter); /* take the absolute value */
            emg1lowfilter = filterlow1.step(emg1abs); /* low pass filtered */
            emg1peak = filterpeak1.step(emg1lowfilter); /* 4dB gain peak */
            
            emg2 = emg2_in.read();      /* read out emg */
            emg2highfilter = filterhigh2.step(emg2); /* high pass filtered */
            emg2notchfilter = filternotch2.step(emg2highfilter); /* notch filtered */
            emg2abs = fabs(emg2notchfilter); /* take the absolute value */
            emg2lowfilter = filterlow2.step(emg2abs); /* low pass filtered */
            emg2peak = filterpeak2.step(emg2lowfilter); /* 4dB gain peak */

            if (max1<emg1peak){
                max1 = emg1peak; /* set the max value at the highest measured value */
            }
            if (max2<emg2peak){
                max2 = emg2peak; /* set the max value at the highest measured value */                
            }
            wait(0.001f); /* measure at 1000Hz */   
            }
            wait(0.2f);
            green = 1;
    }
    maxpart1 = 0.12*max1; /* set cut off voltage at 15% of max for right biceps */
    maxpart2 = 0.12*max2; /* set cut off votage at 15% of max for left biceps */
}

void get_max3(){
    if (max_reader34==0){
            green = 1;
            blue = 1;
            red = !red;
            for(int n=0;n<2000;n++){
                        
            emg3 = emg3_in.read();
            emg3highfilter = filterhigh3.step(emg3);
            emg3notchfilter = filternotch3.step(emg3highfilter);
            emg3abs = fabs(emg3notchfilter);
            emg3lowfilter = filterlow3.step(emg3abs);
            emg3peak = filterpeak3.step(emg3lowfilter);
            
            emg4 = emg4_in.read();
            emg4highfilter = filterhigh4.step(emg4);
            emg4notchfilter = filternotch4.step(emg4highfilter);
            emg4abs = fabs(emg4notchfilter);
            emg4lowfilter = filterlow4.step(emg4abs);
            emg4peak = filterpeak4.step(emg4lowfilter);
            
            if (max3<emg3peak){
                max3 = emg3peak; /* set the max value at the highest measured value */
            }
            if (max4<emg4peak){
                max4 = emg4peak; /* set the max value at the highest measured value */
            }
            wait(0.001f);    
            }
            wait(0.2f);
            red = 1;
    }
    maxpart3 = 0.18*max3; /* set cut off voltage at 25% of max for left lower arm */
    maxpart4 = 0.18*max4; /* set cut off voltage at 25% of max for right lower arm */
}

// Filtering & Scope
void filter() {
    // Right Biceps
    emg1 = emg1_in.read();
    emg1highfilter = filterhigh1.step(emg1);
    emg1notchfilter = filternotch1.step(emg1highfilter);
    emg1abs = fabs(emg1notchfilter);
    emg1lowfilter = filterlow1.step(emg1abs);
    emg1peak = filterpeak1.step(emg1lowfilter); /* Final Right Biceps values to be sent */
    // Left Biceps
    emg2 = emg2_in.read();
    emg2highfilter = filterhigh2.step(emg2);
    emg2notchfilter = filternotch2.step(emg2highfilter);
    emg2abs = fabs(emg2notchfilter);
    emg2lowfilter = filterlow2.step(emg2abs);
    emg2peak = filterpeak2.step(emg2lowfilter); /* Final Left Biceps values to be sent */
    // Left Lower Arm OR Triceps
    emg3 = emg3_in.read();
    emg3highfilter = filterhigh3.step(emg3);
    emg3notchfilter = filternotch3.step(emg3highfilter);
    emg3abs = fabs(emg3notchfilter);
    emg3lowfilter = filterlow3.step(emg3abs);
    emg3peak = filterpeak3.step(emg3lowfilter); /* Final Lower Arm values to be sent */
    // Right Lower Arm OR Triceps
    emg4 = emg4_in.read();
    emg4highfilter = filterhigh4.step(emg4);
    emg4notchfilter = filternotch4.step(emg4highfilter);
    emg4abs = fabs(emg4notchfilter);
    emg4lowfilter = filterlow4.step(emg4abs);
    emg4peak = filterpeak4.step(emg4lowfilter); /* Final Lower Arm values to be sent */  

    
 /* Compare measurement to the calibrated value to decide actions */
 
    /* This part checks for right biceps contractions only*/
if (maxpart1<emg1peak && maxpart2>emg2peak && maxpart3>emg3peak && maxpart4>emg4peak){
            red = 1;
            blue = 1;
            green = 0;
}            
    /* This part checks for left biceps contractions only*/   
else if (maxpart1>emg1peak && maxpart2<emg2peak && maxpart3>emg3peak && maxpart4>emg4peak){
            red = 0;
            blue = 1;
            green = 1;
        }
    /* This part checks for left lower arm contractions only*/   
else if (maxpart1>emg1peak && maxpart2>emg2peak && maxpart3<emg3peak && maxpart4>emg4peak){
            red = 1;
            blue = 0;
            green = 1;
        }
    /* This part checks for right lower arm contractions only */
else if (maxpart1>emg1peak && maxpart2>emg2peak && maxpart3>emg3peak && maxpart4<emg4peak){
            red = 0;
            blue = 1;
            green = 0;
        }           
    /* This part checks for both lower arm contractions only */
else if (maxpart1>emg1peak && maxpart2>emg2peak && maxpart3<emg3peak && maxpart4<emg4peak){
            red = 0;
            blue = 0;
            green = 0;
        }
    /* This part checks for both biceps contractions only*/  
else if (maxpart1<emg1peak && maxpart2<emg2peak && maxpart3>emg3peak && maxpart4>emg4peak){
            red = 0;
            blue = 0;
            green = 0;
        }
    /* This part checks for right lower arm & left biceps contractions only*/
else if (maxpart1>emg1peak && maxpart2<emg2peak && maxpart3>emg3peak && maxpart4<emg4peak){
            red = 0;
            blue = 0;
            green = 0;
        }  
    /* This part checks for left lower arm & right biceps contractions only*/
else if (maxpart1<emg1peak && maxpart2>emg2peak && maxpart3<emg3peak && maxpart4>emg4peak){
            red = 0;
            blue = 0;
            green = 0;
        }                                 
else {
        red = 1; /* Shut down all led colors if no movement is registered */
        blue = 1;
        green = 1;
        //pc.printf( "No contraction registered\n");
    }

    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
    scope.set(0, emg1peak ); /* plot Right biceps voltage */
    scope.set(1, emg2peak ); /* Plot Left biceps voltage */
    scope.set(2, emg3peak ); /* Plot Lower Left Arm voltage */
    scope.set(3, emg4peak ); /* Plot Lower Right Arm Voltage */
 
    scope.send(); /* send everything to the HID scope */
}

int main(){   

    main_timer.attach(&filter, 0.001); /* set frequency for the filters at 1000Hz */
    max_read1.attach(&get_max1, 2); /* set the frequency of the calibration loop at 0.5Hz */
    max_read3.attach(&get_max3, 2);
    while(1) {}
}