File content as of revision 43:a6fbcada47aa:

#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
    //QEI Encoder2(D12, D13, NC, 32); 

    Ticker      main_timer;
    Ticker      max_read1;
    Ticker      max_read3;
    Ticker      Motorcontrol;
    HIDScope    scope( 6 );
    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;
// Moving Average Floats
    // Right Biceps Movavg
    float RB0, RB1, RB2, RB3, RB4, RB5, RB6, RB7, RB8, RB9, RB10, RB11, RB12, 
     RB13, RB14, RB15, RB16, RB17, RB18, RB19, RB20, RB21, RB22, RB23, RB24, 
     RB25, RB26, RB27, RB28, RB29, RB30, RB31, RB32, RB33, RB34, RB35, RB36,
     RB37, RB38, RB39, RB40, RB41, RB42, RB43, RB44, RB45, RB46, RB47, RB48,
     RB49, RB50, RB51, RB52, RB53, RB54, RB55, RB56, RB57, RB58, RB59, RB60,
     RB61, RB62, RB63, RB64, RB65, RB66, RB67, RB68, RB69, RB70, RB71, RB72,
     RB73, RB74, RB75, RB76, RB77, RB78, RB79, RB80, RB81, RB82, RB83, RB84,
     RB85, RB86, RB87, MOVAVG_RB;
    // Left Biceps Movavg
    float LB0, LB1, LB2, LB3, LB4, LB5, LB6, LB7, LB8, LB9, LB10, LB11, LB12, 
     LB13, LB14, LB15, LB16, LB17, LB18, LB19, LB20, LB21, LB22, LB23, LB24, 
     LB25, LB26, LB27, LB28, LB29, LB30, LB31, LB32, LB33, LB34, LB35, LB36,
     LB37, LB38, LB39, LB40, LB41, LB42, LB43, LB44, LB45, LB46, LB47, LB48,
     LB49, LB50, LB51, LB52, LB53, LB54, LB55, LB56, LB57, LB58, LB59, LB60,
     LB61, LB62, LB63, LB64, LB65, LB66, LB67, LB68, LB69, LB70, LB71, LB72,
     LB73, LB74, LB75, LB76, LB77, LB78, LB79, LB80, LB81, LB82, LB83, LB84,
     LB85, LB86, LB87, MOVAVG_LB;
    // Left Lower Arm Movavg 
    float LL0, LL1, LL2, LL3, LL4, LL5, LL6, LL7, LL8, LL9, LL10, LL11, LL12, 
     LL13, LL14, LL15, LL16, LL17, LL18, LL19, LL20, LL21, LL22, LL23, LL24, 
     LL25, LL26, LL27, LL28, LL29, LL30, LL31, LL32, LL33, LL34, LL35, LL36,
     LL37, LL38, LL39, LL40, LL41, LL42, LL43, LL44, LL45, LL46, LL47, LL48,
     LL49, LL50, LL51, LL52, LL53, LL54, LL55, LL56, LL57, LL58, LL59, LL60,
     LL61, LL62, LL63, LL64, LL65, LL66, LL67, LL68, LL69, LL70, LL71, LL72,
     LL73, LL74, LL75, LL76, LL77, LL78, LL79, LL80, LL81, LL82, LL83, LL84,
     LL85, LL86, LL87, MOVAVG_LL;    
    // Right Lower Arm Movavg
    float RL0, RL1, RL2, RL3, RL4, RL5, RL6, RL7, RL8, RL9, RL10, RL11, RL12, 
     RL13, RL14, RL15, RL16, RL17, RL18, RL19, RL20, RL21, RL22, RL23, RL24, 
     RL25, RL26, RL27, RL28, RL29, RL30, RL31, RL32, RL33, RL34, RL35, RL36,
     RL37, RL38, RL39, RL40, RL41, RL42, RL43, RL44, RL45, RL46, RL47, RL48,
     RL49, RL50, RL51, RL52, RL53, RL54, RL55, RL56, RL57, RL58, RL59, RL60,
     RL61, RL62, RL63, RL64, RL65, RL66, RL67, RL68, RL69, RL70, RL71, RL72,
     RL73, RL74, RL75, RL76, RL77, RL78, RL79, RL80, RL81, RL82, RL83, RL84,
     RL85, RL86, RL87, MOVAVG_RL;    
    float AV = 0.02; //multiplication value for adding each movavg value
    // This value also adds a very slight gain to every value
    
// Motor Variables
    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
            RB0 = emg1lowfilter;
            // Movavg is defined by rb0-rb75 and rb1 to rb75 are redefined by the value of rbx-1
            MOVAVG_RB = RB0*AV+RB1*AV+RB2*AV+RB3*AV+RB4*AV+RB5*AV+RB6*AV+RB7*AV+RB8*AV
            +RB9*AV+RB10*AV+RB11*AV+RB12*AV+RB13*AV+RB14*AV+RB15*AV+RB16*AV+RB17*AV+RB18
            *AV+RB19*AV+RB20*AV+RB21*AV+RB22*AV+RB23*AV+RB24*AV+RB25*AV+RB26*AV+RB27
            *AV+RB28*AV+RB29*AV+RB30*AV+RB31*AV+RB32*AV+RB33*AV+RB34*AV+RB35*AV+RB36*AV
            +RB37*AV+RB38*AV+RB39*AV+RB40*AV+RB41*AV+RB42*AV+RB43*AV+RB44*AV+RB45*AV
            +RB46*AV+RB47*AV+RB48*AV+RB49*AV+RB50*AV+RB51*AV+RB52*AV+RB53*AV+RB54*AV
            +RB55*AV+RB56*AV+RB57*AV+RB58*AV+RB59*AV+RB60*AV+RB61*AV+RB62*AV+RB63*AV
            +RB64*AV+RB65*AV+RB66*AV+RB67*AV+RB68*AV+RB69*AV+RB70*AV+RB71*AV+RB72*AV
            +RB73*AV+RB74*AV+RB75*AV+RB76*AV+RB77*AV+RB78*AV+RB79*AV+RB80*AV+RB81*AV
            +RB82*AV+RB83*AV+RB84*AV+RB85*AV+RB86*AV+RB87*AV;
            RB87 = RB86; RB86 = RB85; RB85 = RB84; RB84 = RB83; RB83 = RB82; RB82 = RB81;
            RB81 = RB80; RB80 = RB79; RB79 = RB78; RB78 = RB77; RB77 = RB76; RB76 = RB75;            
            RB75 = RB74; RB74 = RB73; RB73 = RB72; RB72 = RB71; RB71 = RB70; RB70 = RB69; 
            RB69 = RB68; RB68 = RB67; RB67 = RB66; RB66 = RB65; RB65 = RB64; RB64 = RB63; 
            RB63 = RB62; RB62 = RB61; RB61 = RB60; RB60 = RB59; RB59 = RB58; RB58 = RB57; 
            RB57 = RB56; RB56 = RB55; RB55 = RB54; RB54 = RB53; RB53 = RB52; RB52 = RB51; 
            RB51 = RB50; RB50 = RB49; RB49 = RB48; RB48 = RB47; RB47 = RB46; RB46 = RB45; 
            RB45 = RB44; RB44 = RB43; RB43 = RB42; RB42 = RB41; RB41 = RB40; RB40 = RB39; 
            RB39 = RB38; RB38 = RB37; RB37 = RB36; RB36 = RB35; RB35 = RB34; RB34 = RB33; 
            RB33 = RB32; RB32 = RB31; RB31 = RB30; RB30 = RB29; RB29 = RB28; RB28 = RB27; 
            RB27 = RB26; RB26 = RB25; RB25 = RB24; RB24 = RB23; RB23 = RB22; RB22 = RB21; 
            RB21 = RB20; RB20 = RB19; RB19 = RB18; RB18 = RB17; RB17 = RB16; RB16 = RB15; 
            RB15 = RB14; RB14 = RB13; RB13 = RB12; RB12 = RB11; RB11 = RB10; RB10 = RB9; 
            RB9 = RB8; RB8 = RB7; RB7 = RB6; RB6 = RB5; RB5 = RB4; RB4 = RB3; RB3 = RB2; 
            RB2 = RB1; RB1 = RB0;
            
            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
            LB0 = emg2lowfilter;
            // same story as with the right biceps
            MOVAVG_LB = LB0*AV+LB1*AV+LB2*AV+LB3*AV+LB4*AV+LB5*AV+LB6*AV+LB7*AV+LB8*AV
            +LB9*AV+LB10*AV+LB11*AV+LB12*AV+LB13*AV+LB14*AV+LB15*AV+LB16*AV+LB17*AV+LB18
             *AV+LB19*AV+LB20*AV+LB21*AV+LB22*AV+LB23*AV+LB24*AV+LB25*AV+LB26*AV+LB27
            *AV+LB28*AV+LB29*AV+LB30*AV+LB31*AV+LB32*AV+LB33*AV+LB34*AV+LB35*AV+LB36*AV
            +LB37*AV+LB38*AV+LB39*AV+LB40*AV+LB41*AV+LB42*AV+LB43*AV+LB44*AV+LB45*AV
            +LB46*AV+LB47*AV+LB48*AV+LB49*AV+LB50*AV+LB51*AV+LB52*AV+LB53*AV+LB54*AV
            +LB55*AV+LB56*AV+LB57*AV+LB58*AV+LB59*AV+LB60*AV+LB61*AV+LB62*AV+LB63*AV
            +LB64*AV+LB65*AV+LB66*AV+LB67*AV+LB68*AV+LB69*AV+LB70*AV+LB71*AV+LB72*AV
            +LB73*AV+LB74*AV+LB75*AV+LB76*AV+LB77*AV+LB78*AV+LB79*AV+LB80*AV+LB81*AV
            +LB82*AV+LB83*AV+LB84*AV+LB85*AV+LB86*AV+LB87*AV;
            LB87 = LB86; LB86 = LB85; LB85 = LB84; LB84 = LB83; LB83 = LB82; LB82 = LB81;
            LB81 = LB80; LB80 = LB79; LB79 = LB78; LB78 = LB77; LB77 = LB76; LB76 = LB75;
            LB75 = LB74; LB74 = LB73; LB73 = LB72; LB72 = LB71; LB71 = LB70; LB70 = LB69; 
            LB69 = LB68; LB68 = LB67; LB67 = LB66; LB66 = LB65; LB65 = LB64; LB64 = LB63; 
            LB63 = LB62; LB62 = LB61; LB61 = LB60; LB60 = LB59; LB59 = LB58; LB58 = LB57; 
            LB57 = LB56; LB56 = LB55; LB55 = LB54; LB54 = LB53; LB53 = LB52; LB52 = LB51; 
            LB51 = LB50; LB50 = LB49; LB49 = LB48; LB48 = LB47; LB47 = LB46; LB46 = LB45; 
            LB45 = LB44; LB44 = LB43; LB43 = LB42; LB42 = LB41; LB41 = LB40; LB40 = LB39; 
            LB39 = LB38; LB38 = LB37; LB37 = LB36; LB36 = LB35; LB35 = LB34; LB34 = LB33; 
            LB33 = LB32; LB32 = LB31; LB31 = LB30; LB30 = LB29; LB29 = LB28; LB28 = LB27; 
            LB27 = LB26; LB26 = LB25; LB25 = LB24; LB24 = LB23; LB23 = LB22; LB22 = LB21; 
            LB21 = LB20; LB20 = LB19; LB19 = LB18; LB18 = LB17; LB17 = LB16; LB16 = LB15; 
            LB15 = LB14; LB14 = LB13; LB13 = LB12; LB12 = LB11; LB11 = LB10; LB10 = LB9; 
            LB9 = LB8; LB8 = LB7; LB7 = LB6; LB6 = LB5; LB5 = LB4; LB4 = LB3; LB3 = LB2; 
            LB2 = LB1; LB1 = LB0;

            if (max1<MOVAVG_RB){
                max1 = MOVAVG_RB; // set the max value at the highest measured value
            }
            if (max2<MOVAVG_LB){
                max2 = MOVAVG_LB; // set the max value at the highest measured value                
            }
            wait(0.001f); // measure at 1000Hz   
            }
            wait(0.2f);
            green = 1;
    }
    maxpart1 = 0.15*max1; // set cut off voltage at 13% of max for right biceps
    maxpart2 = 0.15*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);
            LL0 = emg3lowfilter;
            // same story as with the right biceps
            MOVAVG_LL = LL0*AV+LL1*AV+LL2*AV+LL3*AV+LL4*AV+LL5*AV+LL6*AV+LL7*AV+LL8*AV
            +LL9*AV+LL10*AV+LL11*AV+LL12*AV+LL13*AV+LL14*AV+LL15*AV+LL16*AV+LL17*AV+LL18
            *AV+LL19*AV+LL20*AV+LL21*AV+LL22*AV+LL23*AV+LL24*AV+LL25*AV+LL26*AV+LL27
            *AV+LL28*AV+LL29*AV+LL30*AV+LL31*AV+LL32*AV+LL33*AV+LL34*AV+LL35*AV+LL36*AV
            +LL37*AV+LL38*AV+LL39*AV+LL40*AV+LL41*AV+LL42*AV+LL43*AV+LL44*AV+LL45*AV
            +LL46*AV+LL47*AV+LL48*AV+LL49*AV+LL50*AV+LL51*AV+LL52*AV+LL53*AV+LL54*AV
            +LL55*AV+LL56*AV+LL57*AV+LL58*AV+LL59*AV+LL60*AV+LL61*AV+LL62*AV+LL63*AV
            +LL64*AV+LL65*AV+LL66*AV+LL67*AV+LL68*AV+LL69*AV+LL70*AV+LL71*AV+LL72*AV
            +LL73*AV+LL74*AV+LL75*AV+LL76*AV+LL77*AV+LL78*AV+LL79*AV+LL80*AV+LL81*AV
            +LL82*AV+LL83*AV+LL84*AV+LL85*AV+LL86*AV+LL87*AV;
            LL87 = LL86; LL86 = LL85; LL85 = LL84; LL84 = LL83; LL83 = LL82; LL82 = LL81;
            LL81 = LL80; LL80 = LL79; LL79 = LL78; LL78 = LL77; LL77 = LL76; LL76 = LL75;
            LL75 = LL74; LL74 = LL73; LL73 = LL72; LL72 = LL71; LL71 = LL70; LL70 = LL69; 
            LL69 = LL68; LL68 = LL67; LL67 = LL66; LL66 = LL65; LL65 = LL64; LL64 = LL63; 
            LL63 = LL62; LL62 = LL61; LL61 = LL60; LL60 = LL59; LL59 = LL58; LL58 = LL57; 
            LL57 = LL56; LL56 = LL55; LL55 = LL54; LL54 = LL53; LL53 = LL52; LL52 = LL51; 
            LL51 = LL50; LL50 = LL49; LL49 = LL48; LL48 = LL47; LL47 = LL46; LL46 = LL45; 
            LL45 = LL44; LL44 = LL43; LL43 = LL42; LL42 = LL41; LL41 = LL40; LL40 = LL39; 
            LL39 = LL38; LL38 = LL37; LL37 = LL36; LL36 = LL35; LL35 = LL34; LL34 = LL33; 
            LL33 = LL32; LL32 = LL31; LL31 = LL30; LL30 = LL29; LL29 = LL28; LL28 = LL27; 
            LL27 = LL26; LL26 = LL25; LL25 = LL24; LL24 = LL23; LL23 = LL22; LL22 = LL21; 
            LL21 = LL20; LL20 = LL19; LL19 = LL18; LL18 = LL17; LL17 = LL16; LL16 = LL15; 
            LL15 = LL14; LL14 = LL13; LL13 = LL12; LL12 = LL11; LL11 = LL10; LL10 = LL9; 
            LL9 = LL8; LL8 = LL7; LL7 = LL6; LL6 = LL5; LL5 = LL4; LL4 = LL3; LL3 = LL2; 
            LL2 = LL1; LL1 = LL0;
            
            emg4 = emg4_in.read();
            emg4highfilter = filterhigh4.step(emg4);
            emg4notchfilter = filternotch4.step(emg4highfilter);
            emg4abs = fabs(emg4notchfilter);
            emg4lowfilter = filterlow4.step(emg4abs);
            RL0 = emg4lowfilter;
            // same story as with the right biceps
            MOVAVG_RL = RL0*AV+RL1*AV+RL2*AV+RL3*AV+RL4*AV+RL5*AV+RL6*AV+RL7*AV+RL8*AV
            +RL9*AV+RL10*AV+RL11*AV+RL12*AV+RL13*AV+RL14*AV+RL15*AV+RL16*AV+RL17*AV+RL18
            *AV+RL19*AV+RL20*AV+RL21*AV+RL22*AV+RL23*AV+RL24*AV+RL25*AV+RL26*AV+RL27
            *AV+RL28*AV+RL29*AV+RL30*AV+RL31*AV+RL32*AV+RL33*AV+RL34*AV+RL35*AV+RL36*AV
            +RL37*AV+RL38*AV+RL39*AV+RL40*AV+RL41*AV+RL42*AV+RL43*AV+RL44*AV+RL45*AV
            +RL46*AV+RL47*AV+RL48*AV+RL49*AV+RL50*AV+RL51*AV+RL52*AV+RL53*AV+RL54*AV
            +RL55*AV+RL56*AV+RL57*AV+RL58*AV+RL59*AV+RL60*AV+RL61*AV+RL62*AV+RL63*AV
            +RL64*AV+RL65*AV+RL66*AV+RL67*AV+RL68*AV+RL69*AV+RL70*AV+RL71*AV+RL72*AV
            +RL73*AV+RL74*AV+RL75*AV+RL76*AV+RL77*AV+RL78*AV+RL79*AV+RL80*AV+RL81*AV
            +RL82*AV+RL83*AV+RL84*AV+RL85*AV+RL86*AV+RL87*AV;
            RL87 = RL86; RL86 = RL85; RL85 = RL84; RL84 = RL83; RL83 = RL82; RL82 = RL81;
            RL81 = RL80; RL80 = RL79; RL79 = RL78; RL78 = RL77; RL77 = RL76; RL76 = RL75;
            RL75 = RL74; RL74 = RL73; RL73 = RL72; RL72 = RL71; RL71 = RL70; RL70 = RL69; 
            RL69 = RL68; RL68 = RL67; RL67 = RL66; RL66 = RL65; RL65 = RL64; RL64 = RL63; 
            RL63 = RL62; RL62 = RL61; RL61 = RL60; RL60 = RL59; RL59 = RL58; RL58 = RL57; 
            RL57 = RL56; RL56 = RL55; RL55 = RL54; RL54 = RL53; RL53 = RL52; RL52 = RL51; 
            RL51 = RL50; RL50 = RL49; RL49 = RL48; RL48 = RL47; RL47 = RL46; RL46 = RL45; 
            RL45 = RL44; RL44 = RL43; RL43 = RL42; RL42 = RL41; RL41 = RL40; RL40 = RL39; 
            RL39 = RL38; RL38 = RL37; RL37 = RL36; RL36 = RL35; RL35 = RL34; RL34 = RL33; 
            RL33 = RL32; RL32 = RL31; RL31 = RL30; RL30 = RL29; RL29 = RL28; RL28 = RL27; 
            RL27 = RL26; RL26 = RL25; RL25 = RL24; RL24 = RL23; RL23 = RL22; RL22 = RL21; 
            RL21 = RL20; RL20 = RL19; RL19 = RL18; RL18 = RL17; RL17 = RL16; RL16 = RL15; 
            RL15 = RL14; RL14 = RL13; RL13 = RL12; RL12 = RL11; RL11 = RL10; RL10 = RL9; 
            RL9 = RL8; RL8 = RL7; RL7 = RL6; RL6 = RL5; RL5 = RL4; RL4 = RL3; RL3 = RL2; 
            RL2 = RL1; RL1 = RL0; 
            
            if (max3<MOVAVG_LL){
                max3 = MOVAVG_LL; // set the max value at the highest measured value
            }
            if (max4<MOVAVG_RL){
                max4 = MOVAVG_RL; // set the max value at the highest measured value
            }
            wait(0.001f);    
            }
            wait(0.2f);
            red = 1;
    }
    maxpart3 = 0.17*max3; // set cut off voltage at 15% of max for left lower arm
    maxpart4 = 0.17*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
    RB0 = emg1lowfilter;
    // Movavg is defined by rb0-rb75 and rb1 to rb75 are redefined by the value of rbx-1
    MOVAVG_RB = RB0*AV+RB1*AV+RB2*AV+RB3*AV+RB4*AV+RB5*AV+RB6*AV+RB7*AV+RB8*AV
    +RB9*AV+RB10*AV+RB11*AV+RB12*AV+RB13*AV+RB14*AV+RB15*AV+RB16*AV+RB17*AV+RB18
    *AV+RB19*AV+RB20*AV+RB21*AV+RB22*AV+RB23*AV+RB24*AV+RB25*AV+RB26*AV+RB27
    *AV+RB28*AV+RB29*AV+RB30*AV+RB31*AV+RB32*AV+RB33*AV+RB34*AV+RB35*AV+RB36*AV
    +RB37*AV+RB38*AV+RB39*AV+RB40*AV+RB41*AV+RB42*AV+RB43*AV+RB44*AV+RB45*AV
    +RB46*AV+RB47*AV+RB48*AV+RB49*AV+RB50*AV+RB51*AV+RB52*AV+RB53*AV+RB54*AV
    +RB55*AV+RB56*AV+RB57*AV+RB58*AV+RB59*AV+RB60*AV+RB61*AV+RB62*AV+RB63*AV
    +RB64*AV+RB65*AV+RB66*AV+RB67*AV+RB68*AV+RB69*AV+RB70*AV+RB71*AV+RB72*AV
    +RB73*AV+RB74*AV+RB75*AV+RB76*AV+RB77*AV+RB78*AV+RB79*AV+RB80*AV+RB81*AV
    +RB82*AV+RB83*AV+RB84*AV+RB85*AV+RB86*AV+RB87*AV;
    RB87 = RB86; RB86 = RB85; RB85 = RB84; RB84 = RB83; RB83 = RB82; RB82 = RB81;
    RB81 = RB80; RB80 = RB79; RB79 = RB78; RB78 = RB77; RB77 = RB76; RB76 = RB75;
    RB75 = RB74; RB74 = RB73; RB73 = RB72; RB72 = RB71; RB71 = RB70; RB70 = RB69; 
    RB69 = RB68; RB68 = RB67; RB67 = RB66; RB66 = RB65; RB65 = RB64; RB64 = RB63; 
    RB63 = RB62; RB62 = RB61; RB61 = RB60; RB60 = RB59; RB59 = RB58; RB58 = RB57; 
    RB57 = RB56; RB56 = RB55; RB55 = RB54; RB54 = RB53; RB53 = RB52; RB52 = RB51; 
    RB51 = RB50; RB50 = RB49; RB49 = RB48; RB48 = RB47; RB47 = RB46; RB46 = RB45; 
    RB45 = RB44; RB44 = RB43; RB43 = RB42; RB42 = RB41; RB41 = RB40; RB40 = RB39; 
    RB39 = RB38; RB38 = RB37; RB37 = RB36; RB36 = RB35; RB35 = RB34; RB34 = RB33; 
    RB33 = RB32; RB32 = RB31; RB31 = RB30; RB30 = RB29; RB29 = RB28; RB28 = RB27; 
    RB27 = RB26; RB26 = RB25; RB25 = RB24; RB24 = RB23; RB23 = RB22; RB22 = RB21; 
    RB21 = RB20; RB20 = RB19; RB19 = RB18; RB18 = RB17; RB17 = RB16; RB16 = RB15; 
    RB15 = RB14; RB14 = RB13; RB13 = RB12; RB12 = RB11; RB11 = RB10; RB10 = RB9; 
    RB9 = RB8; RB8 = RB7; RB7 = RB6; RB6 = RB5; RB5 = RB4; RB4 = RB3; RB3 = RB2; 
    RB2 = RB1; RB1 = RB0;
    // 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
    LB0 = emg2lowfilter;
    // same story as with the right biceps
    MOVAVG_LB = LB0*AV+LB1*AV+LB2*AV+LB3*AV+LB4*AV+LB5*AV+LB6*AV+LB7*AV+LB8*AV
    +LB9*AV+LB10*AV+LB11*AV+LB12*AV+LB13*AV+LB14*AV+LB15*AV+LB16*AV+LB17*AV+LB18
    *AV+LB19*AV+LB20*AV+LB21*AV+LB22*AV+LB23*AV+LB24*AV+LB25*AV+LB26*AV+LB27
    *AV+LB28*AV+LB29*AV+LB30*AV+LB31*AV+LB32*AV+LB33*AV+LB34*AV+LB35*AV+LB36*AV
    +LB37*AV+LB38*AV+LB39*AV+LB40*AV+LB41*AV+LB42*AV+LB43*AV+LB44*AV+LB45*AV
    +LB46*AV+LB47*AV+LB48*AV+LB49*AV+LB50*AV+LB51*AV+LB52*AV+LB53*AV+LB54*AV
    +LB55*AV+LB56*AV+LB57*AV+LB58*AV+LB59*AV+LB60*AV+LB61*AV+LB62*AV+LB63*AV
    +LB64*AV+LB65*AV+LB66*AV+LB67*AV+LB68*AV+LB69*AV+LB70*AV+LB71*AV+LB72*AV
    +LB73*AV+LB74*AV+LB75*AV+LB76*AV+LB77*AV+LB78*AV+LB79*AV+LB80*AV+LB81*AV
    +LB82*AV+LB83*AV+LB84*AV+LB85*AV+LB86*AV+LB87*AV;
    LB87 = LB86; LB86 = LB85; LB85 = LB84; LB84 = LB83; LB83 = LB82; LB82 = LB81;
    LB81 = LB80; LB80 = LB79; LB79 = LB78; LB78 = LB77; LB77 = LB76; LB76 = LB75;
    LB75 = LB74; LB74 = LB73; LB73 = LB72; LB72 = LB71; LB71 = LB70; LB70 = LB69; 
    LB69 = LB68; LB68 = LB67; LB67 = LB66; LB66 = LB65; LB65 = LB64; LB64 = LB63; 
    LB63 = LB62; LB62 = LB61; LB61 = LB60; LB60 = LB59; LB59 = LB58; LB58 = LB57; 
    LB57 = LB56; LB56 = LB55; LB55 = LB54; LB54 = LB53; LB53 = LB52; LB52 = LB51; 
    LB51 = LB50; LB50 = LB49; LB49 = LB48; LB48 = LB47; LB47 = LB46; LB46 = LB45; 
    LB45 = LB44; LB44 = LB43; LB43 = LB42; LB42 = LB41; LB41 = LB40; LB40 = LB39; 
    LB39 = LB38; LB38 = LB37; LB37 = LB36; LB36 = LB35; LB35 = LB34; LB34 = LB33; 
    LB33 = LB32; LB32 = LB31; LB31 = LB30; LB30 = LB29; LB29 = LB28; LB28 = LB27; 
    LB27 = LB26; LB26 = LB25; LB25 = LB24; LB24 = LB23; LB23 = LB22; LB22 = LB21; 
    LB21 = LB20; LB20 = LB19; LB19 = LB18; LB18 = LB17; LB17 = LB16; LB16 = LB15; 
    LB15 = LB14; LB14 = LB13; LB13 = LB12; LB12 = LB11; LB11 = LB10; LB10 = LB9; 
    LB9 = LB8; LB8 = LB7; LB7 = LB6; LB6 = LB5; LB5 = LB4; LB4 = LB3; LB3 = LB2; 
    LB2 = LB1; LB1 = LB0;
    // 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
    LL0 = emg3lowfilter;
    // same story as with the right biceps
    MOVAVG_LL = LL0*AV+LL1*AV+LL2*AV+LL3*AV+LL4*AV+LL5*AV+LL6*AV+LL7*AV+LL8*AV
    +LL9*AV+LL10*AV+LL11*AV+LL12*AV+LL13*AV+LL14*AV+LL15*AV+LL16*AV+LL17*AV+LL18
    *AV+LL19*AV+LL20*AV+LL21*AV+LL22*AV+LL23*AV+LL24*AV+LL25*AV+LL26*AV+LL27
    *AV+LL28*AV+LL29*AV+LL30*AV+LL31*AV+LL32*AV+LL33*AV+LL34*AV+LL35*AV+LL36*AV
    +LL37*AV+LL38*AV+LL39*AV+LL40*AV+LL41*AV+LL42*AV+LL43*AV+LL44*AV+LL45*AV
    +LL46*AV+LL47*AV+LL48*AV+LL49*AV+LL50*AV+LL51*AV+LL52*AV+LL53*AV+LL54*AV
    +LL55*AV+LL56*AV+LL57*AV+LL58*AV+LL59*AV+LL60*AV+LL61*AV+LL62*AV+LL63*AV
    +LL64*AV+LL65*AV+LL66*AV+LL67*AV+LL68*AV+LL69*AV+LL70*AV+LL71*AV+LL72*AV
    +LL73*AV+LL74*AV+LL75*AV+LL76*AV+LL77*AV+LL78*AV+LL79*AV+LL80*AV+LL81*AV
    +LL82*AV+LL83*AV+LL84*AV+LL85*AV+LL86*AV+LL87*AV;
    LL87 = LL86; LL86 = LL85; LL85 = LL84; LL84 = LL83; LL83 = LL82; LL82 = LL81;
    LL81 = LL80; LL80 = LL79; LL79 = LL78; LL78 = LL77; LL77 = LL76; LL76 = LL75;
    LL75 = LL74; LL74 = LL73; LL73 = LL72; LL72 = LL71; LL71 = LL70; LL70 = LL69; 
    LL69 = LL68; LL68 = LL67; LL67 = LL66; LL66 = LL65; LL65 = LL64; LL64 = LL63; 
    LL63 = LL62; LL62 = LL61; LL61 = LL60; LL60 = LL59; LL59 = LL58; LL58 = LL57; 
    LL57 = LL56; LL56 = LL55; LL55 = LL54; LL54 = LL53; LL53 = LL52; LL52 = LL51; 
    LL51 = LL50; LL50 = LL49; LL49 = LL48; LL48 = LL47; LL47 = LL46; LL46 = LL45; 
    LL45 = LL44; LL44 = LL43; LL43 = LL42; LL42 = LL41; LL41 = LL40; LL40 = LL39; 
    LL39 = LL38; LL38 = LL37; LL37 = LL36; LL36 = LL35; LL35 = LL34; LL34 = LL33; 
    LL33 = LL32; LL32 = LL31; LL31 = LL30; LL30 = LL29; LL29 = LL28; LL28 = LL27; 
    LL27 = LL26; LL26 = LL25; LL25 = LL24; LL24 = LL23; LL23 = LL22; LL22 = LL21; 
    LL21 = LL20; LL20 = LL19; LL19 = LL18; LL18 = LL17; LL17 = LL16; LL16 = LL15; 
    LL15 = LL14; LL14 = LL13; LL13 = LL12; LL12 = LL11; LL11 = LL10; LL10 = LL9; 
    LL9 = LL8; LL8 = LL7; LL7 = LL6; LL6 = LL5; LL5 = LL4; LL4 = LL3; LL3 = LL2; 
    LL2 = LL1; LL1 = LL0;
    // 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
    RL0 = emg4lowfilter;
    // same story as with the right biceps
    MOVAVG_RL = RL0*AV+RL1*AV+RL2*AV+RL3*AV+RL4*AV+RL5*AV+RL6*AV+RL7*AV+RL8*AV
    +RL9*AV+RL10*AV+RL11*AV+RL12*AV+RL13*AV+RL14*AV+RL15*AV+RL16*AV+RL17*AV+RL18
    *AV+RL19*AV+RL20*AV+RL21*AV+RL22*AV+RL23*AV+RL24*AV+RL25*AV+RL26*AV+RL27
    *AV+RL28*AV+RL29*AV+RL30*AV+RL31*AV+RL32*AV+RL33*AV+RL34*AV+RL35*AV+RL36*AV
    +RL37*AV+RL38*AV+RL39*AV+RL40*AV+RL41*AV+RL42*AV+RL43*AV+RL44*AV+RL45*AV
    +RL46*AV+RL47*AV+RL48*AV+RL49*AV+RL50*AV+RL51*AV+RL52*AV+RL53*AV+RL54*AV
    +RL55*AV+RL56*AV+RL57*AV+RL58*AV+RL59*AV+RL60*AV+RL61*AV+RL62*AV+RL63*AV
    +RL64*AV+RL65*AV+RL66*AV+RL67*AV+RL68*AV+RL69*AV+RL70*AV+RL71*AV+RL72*AV
    +RL73*AV+RL74*AV+RL75*AV+RL76*AV+RL77*AV+RL78*AV+RL79*AV+RL80*AV+RL81*AV
    +RL82*AV+RL83*AV+RL84*AV+RL85*AV+RL86*AV+RL87*AV;
    RL87 = RL86; RL86 = RL85; RL85 = RL84; RL84 = RL83; RL83 = RL82; RL82 = RL81;
    RL81 = RL80; RL80 = RL79; RL79 = RL78; RL78 = RL77; RL77 = RL76; RL76 = RL75;
    RL75 = RL74; RL74 = RL73; RL73 = RL72; RL72 = RL71; RL71 = RL70; RL70 = RL69; 
    RL69 = RL68; RL68 = RL67; RL67 = RL66; RL66 = RL65; RL65 = RL64; RL64 = RL63; 
    RL63 = RL62; RL62 = RL61; RL61 = RL60; RL60 = RL59; RL59 = RL58; RL58 = RL57; 
    RL57 = RL56; RL56 = RL55; RL55 = RL54; RL54 = RL53; RL53 = RL52; RL52 = RL51; 
    RL51 = RL50; RL50 = RL49; RL49 = RL48; RL48 = RL47; RL47 = RL46; RL46 = RL45; 
    RL45 = RL44; RL44 = RL43; RL43 = RL42; RL42 = RL41; RL41 = RL40; RL40 = RL39; 
    RL39 = RL38; RL38 = RL37; RL37 = RL36; RL36 = RL35; RL35 = RL34; RL34 = RL33; 
    RL33 = RL32; RL32 = RL31; RL31 = RL30; RL30 = RL29; RL29 = RL28; RL28 = RL27; 
    RL27 = RL26; RL26 = RL25; RL25 = RL24; RL24 = RL23; RL23 = RL22; RL22 = RL21; 
    RL21 = RL20; RL20 = RL19; RL19 = RL18; RL18 = RL17; RL17 = RL16; RL16 = RL15; 
    RL15 = RL14; RL14 = RL13; RL13 = RL12; RL12 = RL11; RL11 = RL10; RL10 = RL9; 
    RL9 = RL8; RL8 = RL7; RL7 = RL6; RL6 = RL5; RL5 = RL4; RL4 = RL3; RL3 = RL2; 
    RL2 = RL1; RL1 = RL0; 

    
 // 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<MOVAVG_RB || maxpart2<MOVAVG_LB || maxpart3<MOVAVG_LL || maxpart4<MOVAVG_RL){
    
    if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
            red = 1;
            blue = 1;
            green = 0;
            MV1 = 0.08;
            MV2 = 0;
    }            
    // This part checks for left biceps contractions only  
    else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
            red = 0;
            blue = 1;
            green = 1;
            MV1 = -0.08;
            MV2 = 0;
    }
    // This part checks for left lower arm contractions only  
    else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
            red = 1;
            blue = 0;
            green = 1;
            MV1 = 0;
            MV2 = 0.08;
    }
    // This part checks for right lower arm contractions only
    else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
            red = 0;
            blue = 1;
            green = 0;
            MV1 = 0;
            MV2 = -0.08;
    }           
    // This part checks for both lower arm contractions only 
    else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
            red = 0;
            blue = 0;
            green = 0;
            MV1 = -0.08;
            MV2 = -0.08;
    }
    // This part checks for both biceps contractions only 
    else if (maxpart1<MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
            red = 0;
            blue = 0;
            green = 0;
            MV1 = 0.08;
            MV2 = 0.08;
    }
    // This part checks for right lower arm & left biceps contractions only
    else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
            red = 0;
            blue = 0;
            green = 0;
            MV1 = 0.08;
            MV2 = -0.08;
    }  
    // This part checks for left lower arm & right biceps contractions only
    else if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
            red = 0;
            blue = 0;
            green = 0;
            MV1 = -0.08;
            MV2 = 0.08;
    }
    }                                 
    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, MOVAVG_RB ); // plot Right biceps voltage 
    scope.set(1, MOVAVG_LB ); // Plot Left biceps voltage 
    scope.set(2, MOVAVG_LL ); // Plot Lower Left Arm voltage 
    scope.set(3, MOVAVG_RL ); // 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) {}
}