File content as of revision 24:26659f1de039:

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

//Define objects
    AnalogIn    emg1_in( A0 );
    AnalogIn    emg2_in( A1 );
    AnalogIn    emg3_in( A2 );
    DigitalIn   max_reader1( SW2 );
    DigitalIn   max_reader3( SW3 );


    Ticker      main_timer;
    Ticker      max_read1;
    Ticker      max_read3;
    HIDScope    scope( 5 );
    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 maxpart1;
    // Left Biceps
    double emg2;
    double emg2highfilter;
    double emg2notchfilter;
    double emg2abs;
    double emg2lowfilter;
    double emg2peak;
    double max1;
    double maxpart2;
    // Left Lower Arm OR Triceps
    double emg3;
    double emg3highfilter;
    double emg3notchfilter;
    double emg3abs;
    double emg3lowfilter;
    double emg3peak;
    double max3;
    double maxpart3;

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

// Finding max values for correct motor switch
void get_max1(){
    if (max_reader1==0){
            green = !green;
            red = 1;
            blue = 1;
            for(int n=0;n<2000;n++){
                        
            emg1 = emg1_in.read();
            emg1highfilter = filterhigh1.step(emg1);
            emg1notchfilter = filternotch1.step(emg1highfilter);
            emg1abs = fabs(emg1notchfilter);
            emg1lowfilter = filterlow1.step(emg1abs);
            emg1peak = filterpeak1.step(emg1lowfilter);
            
            if (max1<emg1peak){
                max1 = emg1peak;
            }
            wait(0.001f);    
            }
            wait(0.2f);
            green = 1;
    }
    maxpart1 = 0.28*max1;
    maxpart2 = 0.18*max1;
}

void get_max3(){
    if (max_reader3==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);
            
            if (max3<emg3peak){
                max3 = emg3peak;
            }
            wait(0.001f);    
            }
            wait(0.2f);
            red = 1;
    }
    maxpart3 = 0.28*max3;
}

// 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);
    // 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);
    // 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);
    
    
    /* Compare measurement to the calibrated value to decide actions */
    if (maxpart1<emg1peak){
        red = 0;
        blue = 1;
        green = 1;
        }
    else {
        if (maxpart2<emg2peak){
            red = 1;
            blue = 0;
            green = 1;
            }
        
    else {
        if (maxpart3<emg3peak){
            red = 1;
            blue = 1;
            green = 0;
            }
        
    else {
        red = 1;
        blue = 1;
        green = 1;
        }
        }
        }
    /* 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 );
    scope.set(1, emg2peak );
    scope.set(2, maxpart1 );
    scope.set(3, emg3peak );
    scope.set(4, maxpart3 );    
 
    scope.send();
}

int main(){   

    main_timer.attach(&filter, 0.001);
    max_read1.attach(&get_max1, 2);
    max_read3.attach(&get_max3, 2);
    while(1) {}
}