File content as of revision 31:d346f9244b4a:

#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 );
    
    DigitalOut motor1direction( D4 );
    PwmOut motor1pwm( D5);
    PwmOut motor2pwm( D6 );
    DigitalOut motor2direction( D7 );
    //QEI Encoder1(D10, D11, NC, 32); // Encoder reminder
    //QEI Encoder2(D12, D13, NC, 32); 

    Ticker      main_timer;
    Ticker      max_read1;
    Ticker      max_read3;
    Ticker      Motorcontrol;
    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 max1;
    double maxpart1;
    // Left Biceps
    double emg2;
    double emg2highfilter;
    double emg2notchfilter;
    double emg2abs;
    double emg2lowfilter;
    double max2;
    double maxpart2;
    // Left Lower Arm
    double emg3;
    double emg3highfilter;
    double emg3notchfilter;
    double emg3abs;
    double emg3lowfilter;
    double max3;
    double maxpart3;
    // Right Lower Arm
    double emg4;
    double emg4highfilter;
    double emg4notchfilter;
    double emg4abs;
    double emg4lowfilter;
    double max4;
    double maxpart4;
    
// Motor variables
    float referenceVelocity; 
    float potMeterIn;
    float MV1 = 0;
    float MV2 = 0;
   
// 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 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 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 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 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
// calibration of both biceps

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
            
            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

            if (max1<emg1lowfilter){
                max1 = emg1lowfilter; // set the max value at the highest measured value
            }
            if (max2<emg2lowfilter){
                max2 = emg2lowfilter; // set the max value at the highest measured value                
            }
            wait(0.001f); // measure at 1000Hz   
            }
            wait(0.2f);
            green = 1;
    }
    maxpart1 = 0.11*max1; // set cut off voltage at 13% of max for right biceps
    maxpart2 = 0.13*max2; // set cut off voltage at 13% of max for left biceps
}

// calibration of both lower arms

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);
            
            emg4 = emg4_in.read();
            emg4highfilter = filterhigh4.step(emg4);
            emg4notchfilter = filternotch4.step(emg4highfilter);
            emg4abs = fabs(emg4notchfilter);
            emg4lowfilter = filterlow4.step(emg4abs);
            
            if (max3<emg3lowfilter){
                max3 = emg3lowfilter; // set the max value at the highest measured value
            }
            if (max4<emg4lowfilter){
                max4 = emg4lowfilter; // set the max value at the highest measured value
            }
            wait(0.001f);    
            }
            wait(0.2f);
            red = 1;
    }
    maxpart3 = 0.15*max3; // set cut off voltage at 15% of max for left lower arm
    maxpart4 = 0.15*max4; // set cut off voltage at 15% 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); // 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); // 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); // 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); // Final Lower Arm values to be sent 

    
 // Compare measurement to the calibrated value to decide actions
 // more options could be added if the callibration is well defined enough
    // This part checks for right biceps contractions only
    if (maxpart1<emg1lowfilter && maxpart2>emg2lowfilter && maxpart3>emg3lowfilter && maxpart4>emg4lowfilter){
            red = 1;
            blue = 1;
            green = 0;
            MV1 = 0.5;
            MV2 = 0;
    }            
    // This part checks for left biceps contractions only  
    else if (maxpart1>emg1lowfilter && maxpart2<emg2lowfilter && maxpart3>emg3lowfilter && maxpart4>emg4lowfilter){
            red = 0;
            blue = 1;
            green = 1;
            MV1 = -0.5;
            MV2 = 0;
    }
    // This part checks for left lower arm contractions only  
    else if (maxpart1>emg1lowfilter && maxpart2>emg2lowfilter && maxpart3<emg3lowfilter && maxpart4>emg4lowfilter){
            red = 1;
            blue = 0;
            green = 1;
            MV1 = 0;
            MV2 = 0.5;
    }
    // This part checks for right lower arm contractions only
    else if (maxpart1>emg1lowfilter && maxpart2>emg2lowfilter && maxpart3>emg3lowfilter && maxpart4<emg4lowfilter){
            red = 0;
            blue = 1;
            green = 0;
            MV1 = 0;
            MV2 = -0.5;
    }           
    // This part checks for both lower arm contractions only 
    else if (maxpart1>emg1lowfilter && maxpart2>emg2lowfilter && maxpart3<emg3lowfilter && maxpart4<emg4lowfilter){
            red = 0;
            blue = 0;
            green = 0;
            MV1 = -0.5;
            MV2 = -0.5;
    }
    // This part checks for both biceps contractions only 
    else if (maxpart1<emg1lowfilter && maxpart2<emg2lowfilter && maxpart3>emg3lowfilter && maxpart4>emg4lowfilter){
            red = 0;
            blue = 0;
            green = 0;
            MV1 = 0.5;
            MV2 = 0.5;
    }
    // This part checks for right lower arm & left biceps contractions only
    else if (maxpart1>emg1lowfilter && maxpart2<emg2lowfilter && maxpart3>emg3lowfilter && maxpart4<emg4lowfilter){
            red = 0;
            blue = 0;
            green = 0;
            MV1 = 0.5;
            MV2 = -0.5;
    }  
    // This part checks for left lower arm & right biceps contractions only
    else if (maxpart1<emg1lowfilter && maxpart2>emg2lowfilter && maxpart3<emg3lowfilter && maxpart4>emg4lowfilter){
            red = 0;
            blue = 0;
            green = 0;
            MV1 = -0.5;
            MV2 = 0.5;
    }                                 
    else {
        red = 1; // Shut down all led colors if no movement is registered
        blue = 1;
        green = 1;
        MV1 = 0;
        MV2 = 0;
        //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, emg1lowfilter ); // plot Right biceps voltage 
    scope.set(1, emg2lowfilter ); // Plot Left biceps voltage 
    scope.set(2, emg3lowfilter ); // Plot Lower Left Arm voltage 
    scope.set(3, emg4lowfilter ); // Plot Lower Right Arm Voltage 
    scope.send(); // send everything to the HID scope 
    
}

// check MV1 to see if motor1 needs to be activated
void SetMotor1(float MV1) {
    motor1pwm = fabs(MV1); // motor speed 
    if (MV1 >=0) {
    motor1direction = 1; // clockwise rotation 
    }
    else {
    motor1direction = 0; // counterclockwise rotation 
    }
}
// check MV2 if motor1 needs to be activated
void SetMotor2(float MV2) {
    motor2pwm = fabs(MV2); // motor speed 
    if (MV2 >=0) {
    motor2direction = 1; // clockwise rotation
    }
    else {
    motor2direction = 0; // counterclockwise rotation
    }
}

void MeasureAndControl(void)
{
    // This function measures the potmeter position, extracts a
    // reference velocity from it, and controls the motor with 
    // a simple FeedForward controller. Call this from a Ticker.
    SetMotor1(MV1);
    SetMotor2(MV2);
}

    
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);
    Motorcontrol.attach(MeasureAndControl,0.5);
    while(1) {}
}