File content as of revision 44:8872b20bc1bd:

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

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

// EMG Variables & Constants
    //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

// RKI Variables & Constants
    double setpoint1 = 2.35;
    double setpoint2 = 0.785;
    double x = 10.77;
    double y = 15.73;
    float pi = 3.14159265359;
    float a = 22.25;             //originally 22.25, makes for xinitial=10.766874   // length of arm 1
    float b = 26.5;             //originally 16.48   makes for yinitial=15.733      // length of arm 2
    double alpha_old = 2.35;     //INITIAL ANGLES IN RADIANS
    double beta_old = 0.785;
    float delta1;
    float delta2;
    float diffmotor_a;
    float diffmotor_b;

// PID Variables & Constants
    double Kp = 0.5;// you can set these constants however you like depending on trial & error
    double Ki = 0.1;
    double Kd = 0.1;

    float last_error1 = 0;
    float new_error1 = 0;
    float change_error1 = 0;
    float total_error1 = 0;
    float pid_term1 = 0;
    float pid_term_scaled1 = 0;
    
    float last_error2 = 0;
    float new_error2 = 0;
    float change_error2 = 0;
    float total_error2 = 0;
    float pid_term2 = 0;
    float pid_term_scaled2 = 0;
    
// Motor Variables & Constants
    //float MV1 = 0;
    //float MV2 = 0;
    double count1 = 0;
    double count2 = 0;
    double angle1 = 2.35;
    double angle2 = 0.785;
   
// 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);
    //

// RKI Calculations
//
//

float inversekinematics1(float x, float y){
    // cosine law:
    float c = sqrtf(x*x + y*y);                             // here comes the relevant math, see my inverse kinematics explaination using cosine law
    float alpha1 = acosf((a*a + c*c - b*b)/(2*a*c));
    
    // remaining parts of alpha:
    float alpha2 = atan2f(y,x);
    float alpha = alpha1 + alpha2;
    return alpha;
}

float inversekinematics2(float x, float y){
    // cosine law:
    float c = sqrtf(x*x + y*y);                             // here comes the relevant math, see my inverse kinematics explaination using cosine law
    float beta = acosf((a*a + b*b - c*c)/(2*a*b));
    return beta;
}

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

// EMG Filtering & Scope
//
//
void filter(/*double setpoint1, double setpoint2*/) {
    // 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){
    wait(0.15f);*/
        if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
            red = 1;
            blue = 1;
            green = 0;
            //MV1 = 0.5;
            //MV2 = 0;
            x = x + 0.1;
            if (x > 16) {
            x = 16;
            }
            double alpha = inversekinematics1(x,y); //calculate alpha and beta for current x,y
            double beta = inversekinematics2(x,y);
            setpoint1 = alpha;
            setpoint2 = beta;
            wait(0.1f);
            
        }            
        // 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.5;
            //MV2 = 0;
            x = x - 0.1;
            if (x < 10.77){
            x = 10.87;
            }
            double alpha = inversekinematics1(x,y);
            double beta = inversekinematics2(x,y);
            setpoint1 = alpha;
            setpoint2 = beta;
            wait(0.1f);
        }
        // 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.5;
            y = y + 0.1;
            if (y > 19.5) {
            y = 19.5;
            }
            double alpha = inversekinematics1(x,y);
            double beta = inversekinematics2(x,y);
            setpoint1 = alpha;
            setpoint2 = beta;
            wait(0.1f);
        }
        // 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.5;
            y = y - 0.1;
            if (y < 15.73) {
            y = 15.83;
            }
            double alpha = inversekinematics1(x,y);
            double beta = inversekinematics2(x,y);
            setpoint1 = alpha;
            setpoint2 = beta;
            wait(0.1f);
        }           
/*        // 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.5;
            //MV2 = -0.5;
        }
        // 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.5;
            //MV2 = 0.5;
        }
        // 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.5;
            //MV2 = -0.5;
        }  
        // 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.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;
        }
    
    // 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 
    
}

void encoders(){
        count1 = Encoder1.getPulses();
        count2 = Encoder2.getPulses();
}

/*

// PID calculations
//
//
void PIDcalculation1() {
//PID calculation for motor 1
    filter();
    count1 = Encoder1.getPulses();
    angle1 += (0.000748 * count1);
    new_error1 = setpoint1 - angle1;
    
    change_error1 = new_error1 - last_error1;
    total_error1 += new_error1;
    pid_term1 = (Kp * new_error1) + (Ki * total_error1) + (Kd * change_error1);
    if (pid_term1<-255) {
    pid_term1 = -255;
    }
    if (pid_term1>255) {
    pid_term1 = 255;
    }
    pid_term_scaled1 = abs(pid_term1);
    
    last_error1 = new_error1;
}
void PIDcalculation2() {    
//PID calculation for motor 2
    filter();
    count2 = Encoder2.getPulses();
    angle2 += (0.000748 * count2);
    new_error2 = setpoint2 - angle2;
    
    change_error2 = new_error2 - last_error2;
    total_error2 += new_error2;
    pid_term2 = (Kp * new_error2) + (Ki * total_error2) + (Kd * change_error2);
    if (pid_term2<-255) {
    pid_term2 = -255;
    }
    if (pid_term2>255) {
    pid_term2 = 255;
    }
    pid_term_scaled2 = abs(pid_term2);
    
    last_error2 = new_error2;
}
 */ 
// Motor control
//
//
// check to see if motor1 needs to be activated
void SetMotor1() {
    //PIDcalculation1();
    //filter();
    pc.printf("\n\rBegin SetMotor1\r");
    while (angle1<setpoint1 || angle1>setpoint1 || angle2<setpoint2 || angle2>setpoint2) {
        pc.printf("\n\rlaten we dit dan 0 noemen\r");
        encoders();
        double count1;
        double count2;
        angle1 += (0.0981 * count1);
        angle2 += (0.0981 * count2);
        if (angle1<setpoint1 && angle2<setpoint2) {
        pc.printf("\n\rangle1<setpoint1 && angle2<setpoint2\r");
        motor1direction = 1; // counterclockwise rotation
        motor2direction = 1;
        }
        else if (angle1>setpoint1 && angle2<setpoint2) {
        pc.printf("\n\rangle1>setpoint1 && angle2<setpoint2\r");
        motor1direction = 0; // clockwise rotation
        motor2direction = 1;
        }
        else if (angle1<setpoint1 && angle2>setpoint2) {
        pc.printf("\n\rangle1<setpoint1 && angle2>setpoint2\r");
        motor1direction = 1;
        motor2direction = 0;
        }
        else if (angle1>setpoint1 && angle2>setpoint2) {
        pc.printf("\n\rangle1>setpoint1 && angle2>setpoint2\r");
        motor1direction = 0;
        motor2direction = 0;
        }
        if ((angle1<setpoint1 || angle1>setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {     //1
        pc.printf("\n\rdit noem ik maar 1\r");
        motor1pwm = 0.2;
        motor2pwm = 0.2;
        }
        else if ((angle1<setpoint1 || angle1>setpoint1) && (angle2 == setpoint2)) {
        pc.printf("\n\rdit noem ik maar 2\r");
        motor1pwm = 0.2;
        motor2pwm = 0;
        }
        else if ((angle1 == setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
        pc.printf("\n\rdit noem ik maar 3\r");
        motor1pwm = 0;
        motor2pwm = 0.2;
        }
        else if ((angle1 == setpoint1) && (angle2 == setpoint2)){
        pc.printf("\n\rdit noem ik maar 4\r");
        motor1pwm = 0;
        motor2pwm = 0;
        }
    pc.printf("\n\rend SetMotor1\r");
    }
}
/*
// check if motor1 needs to be activated
void SetMotor2() {
    filter();
    //PIDcalculation2();
    while (angle2<setpoint2 || angle2>setpoint2) {
        count2 = Encoder2.getPulses();
        angle2 += (0.0981 * count2);
        if (angle2<setpoint2){
        motor1direction = 0; // counterclockwise rotation
        }
        else  if (angle2>setpoint2){
        motor1direction = 1; // clockwise rotation
        }
        if (angle2<setpoint2 || angle2>setpoint2) {
        motor2pwm = 0.5;
        }
        else if (angle2 == setpoint2){
        motor2pwm = 0;
        }
    }
}

void MeasureAndControl(void) {
    SetMotor1();
    //SetMotor2();
}
*/
    
int main(){   
 // pc.baud(115200);
    pc.printf("\n\rMAIN START\r");
    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);
    tencoder.attach(&encoders, 0.001);
    Motorcontrol.attach(&SetMotor1,0.1);
    //PIDtimer.attach(&PIDcalculation1, 0.005);
    //PIDtimer.attach(&PIDcalculation2, 0.005);
    pc.printf("\n\rMAIN END!!!");
    while(1) {}
}