File content as of revision 45:653370fa8b67:

#include "mbed.h"
#include "QEI.h"
#include "HIDScope.h"
#include "Biquad.h"
#include "controlandadjust.h"
#include "angleandposition.h"

///////////////////////////////////////////////info out
HIDScope scope(6);// number of hidscope channels
Ticker scope_ticker;//ticker for the scope
const double scope_frequency=200; //HIDscope frequency

Timer checktimer;// timer to check how much time it takes to run the control loop, to see if the frequencies are not too high
volatile double checktimervalue=0; // make a variable to store the time the loop has taken

Serial pc(USBTX,USBRX);// serial connection to pc

DigitalOut ledred(LED_RED);// leds on the k64f board
DigitalOut ledgreen(LED_GREEN);
DigitalOut ledblue(LED_BLUE);


/////////////////////////////////////////////ENCODERS
const double cpr_sensor=32; //counts per rotation of the sensor
const double cpr_shaft=cpr_sensor*131;//counts per rotation of the outputshaft

QEI encoder1(D13,D12,NC,cpr_sensor);/// encoders on motors X2 encoding
QEI encoder2(D10,D11,NC,cpr_sensor);// first is pin a, pin b and the second is pin b, pin a so the encoders give positive rotation when the pod is moved forward

const double PIE=3.14159265359; // pi, for calculations
const double counttorad=((2*PIE)/cpr_shaft);// to convert counts to rotation in rad


/////////////////////////////////CALIBRATION (MODE)
const double radpersec_calibrate=0.1*PIE;// speed of arms when in calibration mode
int modecounter=1;//counter in which mode the robot is
const double readbuttoncalibrate_frequency=50;//frequency at which the buttons are read when in calibration mode
const double ledblink_frequency=4;//frequency at which the green led and leds on top blink when in resp button or calibration mode

InterruptIn changemodebutton(PTA4);// button to change mode (sw3)
Ticker readbuttoncalibrate_ticker;//ticker for reading out the buttons when calibrating
Ticker ledblink_ticker;// ticker for blinking the leds

DigitalIn buttonR(D2);//rigth button on biorobotics shield
DigitalIn buttonL(D3);//left button on biorobotics shield

/////////////////READSIGNAL
const double readsignal_frequency=100;//frequency at wich the filtered emg signal is sampled to be 0 or 1
Ticker readsignal_ticker; // ticker for reading the signal of the emg


DigitalOut led1(PTC12);// rigth led on biorobotics shield
DigitalOut led2(D9);//left led on biorobotics shield

//////////////////////////////////CONTROLLER
const double control_frequency=250;// frequency at which the controller is called
//controller constants
float Kp_shoot=6;
float Ki_shoot=2;
float Kd_shoot=0.5;
float Kp_move=2;
float Ki_move=1.25;
float Kd_move=0.05;

controlandadjust mycontroller(2,control_frequency); // make a controller, value in brackets is errorband in degrees and controller frequency

Ticker control_ticker;//ticker for the controller
const double Ts_control=1.0/control_frequency;//sample time of the controller

double error1=0,//controller error storage variables
       error2=0;

InterruptIn valuechangebutton(PTC6);//button to change filter gains per arm via Serial

//safetyandthreshold
AnalogIn safety_pot(A3);//pot 2, used for the safety cutoff value for the pwm
AnalogIn threshold_pot(A2);//pot1, used to adjust threshold if signal differs per person

Ticker safetyandthreshold_ticker; // ticker to read potmeters
const double safetyandthreshold_frequency=1; // frequency for the ticker

double threshold_value=1;//initial threshold value

////////////////////////////////FILTER
const double filter_frequency=500; // frequency at which the emg signal is filtered
#include "filtervalues.h"// call the values for the biquads these are in a separate file to keep this code readable

Ticker filter_ticker;//Ticker for the filter

Biquad myfilter1;// make filter for signal 1
Biquad myfilter2;//make filter for signal 2

AnalogIn emg1_input(A0);//input for first emg signal
AnalogIn emg2_input(A1);//input for second emg signal

volatile double filteredsignal1=0;//the first filtered emg signal
volatile double filteredsignal2=0;//the second filtered emg signal
double filter_extragain1=1,filter_extragain2=1; // this is a factor to increase the gain per arm, adustable via serial


//////////////////////////////// POSITION AND ANGLE
const double safetymarginfield=0.05; //adjustable, tweak for maximum but safe range
const double mm_per_sec_emg=0.1;// move the pod 100 mm per sec if muscle is flexed
const double y_start=0.225;//starting y position of the pod
const double y_punch=0.473;// position to where there is punched
const double timetoshoot=0.35;// time it can take to shoot
const double timetogoback=1;// time it can take to go back after shooting

volatile double desired_position=0;//desired x position
double desired_angle1=0; //desired angle of arm 1 (calculated with anglepos)
double desired_angle2=0; // desired anvle of arm 2 (calculated with anglepos)

const double fieldwidth=0.473; // width of the field
const double maxdisplacement=((fieldwidth/2)-safetymarginfield); //so the pod doesn't hit the edges of the playfield

angleandposition anglepos;// initiate the angle and position calculation library

const double radtodeg=(180/PIE); // to convert radians to degrees


/////////////////////////////////////////////////////////DEMO MODE
const double demo_safety=0.05;// safety factor to stay off the edges
const double demo_x_edge_rigth=(fieldwidth/2)-demo_safety;// rigth edge of the field
const double demo_x_edge_left=-((fieldwidth/2)-demo_safety);// left edge of te field
const double demo_goal_edge_rigth=0.1+demo_safety;// x edges of goal
const double demo_goal_edge_left=-(0.1+demo_safety);
const double demo_y_goal=0.1+demo_safety;// y position in front of goal
const double demo_y_back=0.05+demo_safety;//back edge of the field
const double demo_y_front=y_punch;

int demo_go=0;



//////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
volatile bool scopedata_go=false,
              control_go=false,
              filter_go=false,
              safetyandthreshold_go=false,
              readsignal_go=false,
              switchedmode=true,
              readbuttoncalibrate_go=false,
              ledblink_go=false,
              demo_next_step_go=false;

void scopedata_activate()
{
    scopedata_go=true;
}
void control_activate()
{
    control_go=true;
}
void filter_activate()
{
    filter_go=true;
}
void safetyandthreshold_activate()
{
    safetyandthreshold_go=true;
}
void readsignal_activate()
{
    readsignal_go=true;
}
void readbuttoncalibrate_activate()
{
    readbuttoncalibrate_go=true;
}
void ledblink_activate()
{
    ledblink_go=true;
}
void demo_next_step_activate()
{
    demo_next_step_go=true;
}

////////////////////////FUNCTIONS
//gather data and send to scope
void scopedata(double wanted_y)
{
    scope.set(0,desired_position); // desired x position
    scope.set(1,wanted_y); // desired y position
    scope.set(2,filteredsignal1); // filterded emg signal rigth arm
    scope.set(3,filteredsignal2); // filtered emg signal left arm
    scope.set(4,threshold_value); // threshold value
    scope.set(5,mycontroller.motor1pwm()); //pwm signal send to motor 1
    scope.send(); // send info to HIDScope server
}
//read potmeters and adjust the safetyfactor and threshold
void safetyandthreshold()
{
    mycontroller.cutoff((ceil (10*safety_pot.read()) )/10); // adjust the safetyfactor value between 0 and 1 rounded to 1 decimal
    threshold_value=((ceil (100*threshold_pot.read()) )/100); // adjust the threshold value between 0 and 1 rounded to 2 decimals
}
/////filter
void filtereverything(bool makeempty)
{
    //pass1 so f1
    double pass1_emg1 = myfilter1.filter(emg1_input.read(), v1_f1_emg1 , v2_f1_emg1 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1);
    double pass1_emg2 = myfilter2.filter(emg2_input.read(), v1_f1_emg2 , v2_f1_emg2 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1);

    //pass2 so f2
    double pass2_emg1 = myfilter1.filter(pass1_emg1, v1_f2_emg1 , v2_f2_emg1 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2);
    double pass2_emg2 = myfilter2.filter(pass1_emg2, v1_f2_emg2 , v2_f2_emg2 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2);

    //pass3 so f3
    double pass3_emg1 = myfilter1.filter(pass2_emg1, v1_f3_emg1 , v2_f3_emg1 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3);
    double pass3_emg2 = myfilter2.filter(pass2_emg2, v1_f3_emg2 , v2_f3_emg2 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3);

    //pass4 so f4
    double pass4_emg1 = myfilter1.filter(pass3_emg1, v1_f4_emg1 , v2_f4_emg1 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4);
    double pass4_emg2 = myfilter2.filter(pass3_emg2, v1_f4_emg2 , v2_f4_emg2 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4);

    //pass5 so f5
    double pass5_emg1 = myfilter1.filter(pass4_emg1, v1_f5_emg1 , v2_f5_emg1 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5);
    double pass5_emg2 = myfilter2.filter(pass4_emg2, v1_f5_emg2 , v2_f5_emg2 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5);

    ///// take absolute value
    double pass5_emg1_abs=(fabs(pass5_emg1));
    double pass5_emg2_abs=(fabs(pass5_emg2));

    //pass6 so f6
    double pass6_emg1 = myfilter1.filter(pass5_emg1_abs, v1_f6_emg1 , v2_f6_emg1 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6);
    double pass6_emg2 = myfilter2.filter(pass5_emg2_abs, v1_f6_emg2 , v2_f6_emg2 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6);


    //pass7 so f7
    double pass7_emg1 = myfilter1.filter(pass6_emg1, v1_f7_emg1 , v2_f7_emg1 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);
    double pass7_emg2 = myfilter2.filter(pass6_emg2, v1_f7_emg2 , v2_f7_emg2 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);

    filteredsignal1=(pass7_emg1*9e11*filter_extragain1);//this is needed to make the signal ~1 when flexed
    filteredsignal2=(pass7_emg2*9e11*filter_extragain2);// filter_extragain is adjusted via serial

    if (makeempty==true) {//this is needed so the filtered value is not high after shooting basically it resets the filter
        pass1_emg1 = pass1_emg2 =pass2_emg1 =pass2_emg2 =pass3_emg1 = pass3_emg2 =pass4_emg1 =pass4_emg2 =pass5_emg1 =pass5_emg2 =0;
        pass5_emg1_abs=pass5_emg2_abs=pass6_emg1 =pass6_emg2 =pass7_emg1=pass7_emg2 =filteredsignal1=filteredsignal2=0;
        v1_f1_emg1=v1_f1_emg2=v1_f2_emg1=v1_f2_emg2=v1_f3_emg1=v1_f3_emg2=v1_f4_emg1=v1_f4_emg2=v1_f5_emg1=0;
        v1_f5_emg2=v1_f6_emg1=v1_f6_emg2=v1_f7_emg1=v1_f7_emg2=0;
    }

}
//adjust controller values (and wheter or not demo is shown) when sw2 is pressed
void valuechange()
{
    if (modecounter==4) {// cannot be changed if demo loop is running
        pc.printf("Not now!\n");
    } else {
        mycontroller.STOP(); // stop motors
        pc.printf("Extra gain for rigth arm is now %f, enter new value\n",filter_extragain1); // print extra gain for rigth arm
        pc.scanf("%f", &filter_extragain1); //read the input from serial and write to filter_extragain1

        pc.printf("Extra gain for left arm is now %f, enter new value\n",filter_extragain2); // print extra gain for left arm
        pc.scanf("%f", &filter_extragain2);//read the input from serial and write to filter_extragain2

        pc.printf("Do you want to enter demo mode after button control mode 1/0? (1 for yes, 0 for no.\n");
        pc.scanf("%i", &demo_go);//read the input from serial and write to filter_extragain2

        pc.printf("Done\n");        
    }
}

// shoot the pod forward
void shoot()
{
    ledgreen=1;
    double time=0;
    double stepsize=(0.5*PIE)/(timetoshoot*control_frequency); //calculate stepsize
    double profile_angle=0;// used for the profilie
    double x_punch=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());// calculate actual x positon from the encoder angles
    double y_during_punch=y_start;//set initial y value to baseline
    Timer shoottimer; // make a timer
    shoottimer.reset();
    shoottimer.start();
    //forward
    while (time<=timetoshoot) {
        ledblue=!ledblue; // blink blue led (very fast, so not noticable blinking)

        profile_angle+=stepsize; // add stepsize to angle

        y_during_punch=(y_punch-y_start)*(1-cos(profile_angle))+y_start; // calculate y position for shooting

        if (y_during_punch>=y_punch) {//to check if y position is not bigger than y_punch for safety
            y_during_punch=y_punch;
        } else {
            y_during_punch=y_during_punch;
        }

        desired_angle1=anglepos.positiontoangle1(x_punch,y_during_punch);// calculate desired angles
        desired_angle2=anglepos.positiontoangle2(x_punch,y_during_punch);

        error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
        error2=(desired_angle2-counttorad*encoder2.getPulses());

        mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller
        scopedata(y_during_punch);//send data to hidscope   WARING lower freqyency than normal

        time+=(Ts_control);// add time it should take to calculated time
        filtereverything(true);//set al filter variables to 0

        wait(time-shoottimer.read());// IMPORTANT wait until the loop has taken the time it should need,
        // if this is not done the loop wil go to fast and the motors can't keep up
        // this is because the control loop takes far less time than Ts_control
    }
    //back
    time=0; // reset time
    shoottimer.reset();//reset timer
    stepsize=(PIE)/(timetogoback*control_frequency);//calculate stepsize
    profile_angle=0;//reset angle for the displacement profile
    desired_position=0;// set desired x position to 0, so the robot is in the middle in front of the goal after shooting
    while (time<=timetogoback) {
        ledblue=!ledblue;// blink blue led

        profile_angle+=stepsize; // add stepsize to y position
        y_during_punch=0.5*(y_punch-y_start)*(1+cos(profile_angle))+y_start; // calculate y position for shooting
        if (y_during_punch<=y_start) {//to check if y position is not smaller than y_start for safety
            y_during_punch=y_start;
        } else {
            y_during_punch=y_during_punch;
        }

        desired_angle1=anglepos.positiontoangle1(desired_position,y_during_punch);// calculate desired angles
        desired_angle2=anglepos.positiontoangle2(desired_position,y_during_punch);


        error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
        error2=(desired_angle2-counttorad*encoder2.getPulses());

        mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller
        scopedata(y_during_punch);//send data to hidscope   WARING lower freqyency than normal

        time+=(Ts_control);// add time it should take to calculated time
        filtereverything(false);//start filtering the signal (lower frewquency than normal)
        wait(time-shoottimer.read());// IMPORTANT wait until the loop has taken the time it should need, if this is not done the loop wil go to fast and the motors can't keep up
    }
    shoottimer.stop();
    ledblue=1; // blue led off
    ledgreen=0; // green led on
}

////////////////////////////////////////////////////READ EMG AND MOVE DESIRED POSITION
void readsignal()
{
    //check if pod has to shoot
    if (filteredsignal1>=threshold_value && filteredsignal2>=threshold_value) {
        led1=led2=1; // ligth up  both leds
        shoot();
        // check if pod has to move to the right
    } else if (filteredsignal1>=threshold_value && filteredsignal2<=threshold_value) {
        led1=1;// ligth up rigth led
        led2=0;
        desired_position += (mm_per_sec_emg/readsignal_frequency);// move desiredposition right
        if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge
            desired_position=maxdisplacement;
        } else {
            desired_position=desired_position;
        }
        // check if pod has to move to the left
    } else if (filteredsignal1<=threshold_value && filteredsignal2>=threshold_value) {
        led1=0; // ligth up left led
        led2=1;
        desired_position -= (mm_per_sec_emg/readsignal_frequency);//move desiredposition left
        if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge
            desired_position=(-1*maxdisplacement);
        } else {
            desired_position=desired_position;
        }
    } else {
        led1=led2=0;
    }
}

///////////////////////////////////////////////READ BUTTON AND MOVE DESIRED POSITION
void readsignalbutton() // same as above, only with buttons as input
{
    //write value of button to variable
    int buttonr=buttonR.read();
    int buttonl=buttonL.read();
    //check if pod has to shoot
    if (buttonr==0 && buttonl==0) {
        led1=led2=1;
        shoot();
        // check if pod has to move to the right
    } else if (buttonr==0 && buttonl==1) {
        led1=1;
        led2=0;
        desired_position += (mm_per_sec_emg/readsignal_frequency);// move desiredposition right
        if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge
            desired_position=maxdisplacement;
        } else {
            desired_position=desired_position;
        }
        // check if pod has to move to the left
    } else if (buttonr==1 && buttonl==0) {
        led1=0;
        led2=1;
        desired_position -= (mm_per_sec_emg/readsignal_frequency);//move desiredposition left
        if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge
            desired_position=(-1*maxdisplacement);
        } else {
            desired_position=desired_position;
        }
    } else {
        led1=led2=0;
    }
}

void changemode()   //this makes the counter higher to switch between modes
{
    mycontroller.STOP(); // stop the controller
    switchedmode=true; // set switchedmode to true so in the main loop some statements are executed
    modecounter++; // increase counter
    if (modecounter==6) { // reset counter if counter=6
        modecounter=0;
    } else {
        modecounter=modecounter;
    }

}

// function takes x,y and time as input and moves pod to the desired coordinates in the desired time
void gotopos(double desired_x_pos, double desired_y_pos, double time_to_pos)
{
    double timepos=0;
    Timer postimer;
    double pos_x_start=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());
    double pos_y_start=anglepos.angletoposition_y(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());

    double pos_stepsize_x=(pos_x_start-desired_x_pos)/(time_to_pos*control_frequency);
    double pos_stepsize_y=(pos_y_start-desired_y_pos)/(time_to_pos*control_frequency);

    double pos_x_moving=pos_x_start;
    double pos_y_moving=pos_y_start;

    postimer.start();
    while(timepos<=time_to_pos) {
        pos_x_moving-=pos_stepsize_x;
        pos_y_moving-=pos_stepsize_y;

        desired_angle1=anglepos.positiontoangle1(pos_x_moving,pos_y_moving);// calculate desired angles
        desired_angle2=anglepos.positiontoangle2( pos_x_moving,pos_y_moving);

        error1=(desired_angle1-counttorad*encoder1.getPulses());//calculate errors
        error2=(desired_angle2-counttorad*encoder2.getPulses());

        mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller

        timepos+=(Ts_control);// add time it should take to calculated time
        wait(timepos-postimer.read());// IMPORTANT wait until the loop has taken the time it should need, if this is not done the loop wil go to fast and the motors can't keep up
    }
}

///////////////////////////////////////////////////MAIN

int main()
{
    //initiate tickers
    safetyandthreshold_ticker.attach(&safetyandthreshold_activate,1.0/safetyandthreshold_frequency);
    filter_ticker.attach(&filter_activate,1.0/filter_frequency);
    control_ticker.attach(&control_activate,1.0/control_frequency);
    scope_ticker.attach(&scopedata_activate,1.0/scope_frequency);
    readsignal_ticker.attach(&readsignal_activate, 1.0/readsignal_frequency);
    readbuttoncalibrate_ticker.attach(&readbuttoncalibrate_activate, 1.0/readbuttoncalibrate_frequency);
    ledblink_ticker.attach(&ledblink_activate, 1.0/ledblink_frequency);

    pc.baud(115200);//set baudrate to 115200

    while(1) {// while (1) so continious loop
        valuechangebutton.fall(&valuechange);// used to change the filter gains
        changemodebutton.rise(&changemode);// interruptin for next mode
        checktimer.reset();// reset the timer to check the time it takes to run the entire control loop
        checktimer.start();

        if (scopedata_go==true) {//send scopedata
            //TIME THIS LOOP TAKES: 0.000008 SEC (PEAKS AT 0.000015)
            scopedata(y_start);// call scopedata, use baseline as desired y position
            scopedata_go=false;
        }
        if (safetyandthreshold_go==true) {// check the potmeters
            //TIME THIS LOOP TAKES: 0.000032 SEC
            safetyandthreshold(); // read out the potmeters on top of the biorobotics board
            safetyandthreshold_go=false;
        }
        ///////////////////////////////////////////NORMAL RUNNING MODE
        if(modecounter==0) {
            if (switchedmode==true) {
                pc.printf("Program running\n");// print to serial
                led1=led2=ledgreen=0;// green led on leds on top off
                ledred=ledblue=1;//rest off
                gotopos(0,y_start,1);
                desired_position=0;
                switchedmode=false;
            }
            if (filter_go==true) {// filter the emg signal
                // TIME THIS LOOP TAKES: 0.000173 SEC
                filtereverything(false);
                filter_go=false;
            }
            if (readsignal_go==true) {// check if signal is 0 or 1 and adjust wanted position
                // TIME THIS LOOP TAKES: 0.000005 SEC
                readsignal();
                readsignal_go=false;
            }
            if (control_go==true) {// calculate angles from positions and send error to controller
                //TIME THIS LOOP TAKES: 0.000223 SEC

                desired_angle1=anglepos.positiontoangle1(desired_position,y_start);
                desired_angle2=anglepos.positiontoangle2(desired_position,y_start);

                double error1=(desired_angle1-counttorad*encoder1.getPulses());
                double error2=(desired_angle2-counttorad*encoder2.getPulses());
                mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move);
                control_go=false;
            }
        }
        ////////////////////////////////////////////////////CALIBRATE RIGHT ARM
        if (modecounter==1) {
            if(switchedmode==true) {
                pc.printf("Calibration mode! Use buttons to move rigth arm to 0 degrees\n");
                led1=led2=ledred=0;
                ledgreen=ledblue=1;
                switchedmode=false;
            }
            if (ledblink_go==true) {
                led1=!led1;// blink rigth led on biorobotics shield (because rigth arm is being calibrated)
                ledblink_go=false;
            }
            if (readbuttoncalibrate_go==true) {//check wich button is pressed and adjust wanted angle of rigth arm
                if (buttonR.read()==0 && buttonL.read()==1) {
                    desired_angle1 += (radpersec_calibrate/readbuttoncalibrate_frequency);
                    readbuttoncalibrate_go=false;
                }
                if (buttonR.read()==1 && buttonL.read()==0) {
                    desired_angle1 -= (radpersec_calibrate/readbuttoncalibrate_frequency);
                    readbuttoncalibrate_go=false;
                }
            }
            if (control_go==true) {// calculate errors and send them to controllers
                error1=(desired_angle1-counttorad*encoder1.getPulses());
                error2=0;// only adjust rigth arm
                mycontroller.PI(error1,error2,Kp_move,Ki_move);
                control_go=false;
            }
        }
        ////////////////////////////////////////////CALIBRATE LEFT ARM
        if (modecounter==2) {
            if(switchedmode==true) {
                pc.printf("Calibration mode! Use buttons to move left arm to 0 degrees\n");
                led1=led2=ledred=0;
                ledgreen=ledblue=1;
                switchedmode=false;
            }
            if (ledblink_go==true) {
                led2=!led2;// blink left led on biorobotics shield (because left arm is being calibrated)
                ledblink_go=false;
            }
            if (readbuttoncalibrate_go==true) {//
                if (buttonR.read()==0 && buttonL.read()==1) {//check wich button is pressed and adjust wanted angle of left arm
                    desired_angle2 += (radpersec_calibrate/readbuttoncalibrate_frequency);
                    readbuttoncalibrate_go=false;
                }
                if (buttonR.read()==1 && buttonL.read()==0) {
                    desired_angle2 -= (radpersec_calibrate/readbuttoncalibrate_frequency);
                    readbuttoncalibrate_go=false;
                }
            }
            if (control_go==true) {// calculate errors and send to controller
                error1=0; // only adjus left arm
                error2=(desired_angle2-counttorad*encoder2.getPulses());
                mycontroller.PI(error1,error2,Kp_move,Ki_move);
                control_go=false;
            }
        }
        ///////////////////////////////BUTTONCONTROLMODE
        if (modecounter==3) {
            if (switchedmode==true) {
                pc.printf("Buttonmode, you can use the buttons to control the robot\n");
                led1=led2=0;
                ledred=ledblue=1;
                encoder1.reset();// reset encoders so they are at 0 degrees
                encoder2.reset();
                desired_position=0;//set desired position to the middle of the field, where the pod actually is.
                switchedmode=false;
            }
            if (ledblink_go==true) {
                ledgreen=!ledgreen; // blink green led
                ledblink_go=false;
            }
            if (readsignal_go==true) {// read buttons and adjust wanted position
                readsignalbutton();
                readsignal_go=false;
            }
            if (control_go==true) {// calculate wanted angles from position, errors and send to controller
                desired_angle1=anglepos.positiontoangle1(desired_position,y_start);
                desired_angle2=anglepos.positiontoangle2(desired_position,y_start);

                error1=(desired_angle1-counttorad*encoder1.getPulses());
                error2=(desired_angle2-counttorad*encoder2.getPulses());
                mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move);
                control_go=false;
            }
        }
        ///////////////////////////////////DEMO MODE
        if  (modecounter==4 && demo_go==!1) {//if demo mode not enabled, move to next mode
            changemode();
        }
        if (modecounter==4 && demo_go==1) {
            if (switchedmode==true) {
                pc.printf("Demo mode, the pod moves around the field\n");
                led1=led2=ledred=ledblue=ledgreen=0; // rgb on, leds on top off
                gotopos(0,demo_y_goal,1); // go to goal @x=0
                switchedmode=false;
            }

            // loop trough positions to make a path along the field, con of this method is that the mode is only changed when the loop is completed.
            // the button is read, but only when the loop is completed it is checked in what mode the robot has to be, that's why this mode is made optional,
            // because this mode is only neccesary for the presentation on 30-10-2015 (or for fun)
            gotopos(demo_goal_edge_left,demo_y_goal,0.5);// in front of goal to left edge of goal
            gotopos(demo_goal_edge_left,demo_y_back,0.25); // to back along left egde of goal
            gotopos(demo_x_edge_left,demo_y_back,0.5); //to left edge along back
            gotopos(demo_x_edge_left,demo_y_front,2);//from back to front along left edge
            gotopos(demo_x_edge_rigth,demo_y_front,2);// from left to rigth along front edge
            gotopos(demo_x_edge_rigth,demo_y_back,2);// from front to back along rigth edge
            gotopos(demo_goal_edge_rigth,demo_y_back,0.5);//from rigth edge to rigth edge of goal along back
            gotopos(demo_goal_edge_rigth,demo_y_goal,0.25); // from back to in front of goal along rigth edge goal
            gotopos(0,demo_y_goal,0.5); // from rigth edge goal to middle in front of goal
            gotopos(0,y_start,0.5);
            demo_go=0;
        }

        //////////////////////////////////EMG GAIN AND THRESHOLD CALIBRATION MODE
        if(modecounter==5) {
            if (switchedmode==true) {
                pc.printf("Calibrate the EMG signals and threshold\n");
                ledblue=ledgreen=1;
                led1=led2=ledred=0;
                switchedmode=false;
            }
            if(ledblink_go==true) {
                ledgreen=!ledgreen;
                ledred=!ledred;
                ledblink_go=false;
            }
            if (filter_go==true) {// filter the emg signal
                // TIME THIS LOOP TAKES: 0.000173 SEC
                filtereverything(false);
                filter_go=false;
            }
        }
        checktimervalue=checktimer.read(); // write the time it has taken to a variable, so this variable can be displayed using hidschope.
        // if chectimer.read() is used in scopedata, the value is probably ~0
        //because scopedata is called as one of the first functoins
        checktimer.stop();
    }
}