Diff: main.cpp

Revision:

27:e704fdc41e87

Parent:

26:418f025a30c6

--- a/main.cpp	Fri Nov 01 17:18:39 2019 +0000
+++ b/main.cpp	Mon Nov 04 14:47:17 2019 +0000
@@ -1,5 +1,3 @@
-// Operating mode might not go to next state when SW2 is pressed
-
 #include "mbed.h"
 #include "HIDScope.h"
 #include "BiQuad.h"
@@ -11,49 +9,58 @@
 
 #include "Servo.h"  // Included to control the servo motor 
 
-Servo myservo(D13);     // To control the servo motor
-DigitalIn but3(SW2);    // To go to the next state or to choose one of two game modes when in state START_GAME
-DigitalIn but4(SW3);    // To choose one of two game modes when in state START_GAME or to move the gripper
-DigitalOut ledr(LED_RED);
+Servo myservo(D13);         // To control the servo motor
+
+DigitalIn button1(SW2);     // Button to go to the next state or to choose one of two game modes when in state START_GAME
+DigitalIn button2(SW3);     // Button to choose one of two game modes when in state START_GAME or to move the gripper in the operating mode
+
+DigitalOut ledr(LED_RED);   // Define the output for the three different LED's on the K64F. These will be used to show in which state the robot currently is in
 DigitalOut ledg(LED_GREEN);
 DigitalOut ledb(LED_BLUE);
 
-AnalogIn S0(A0);
+AnalogIn S0(A0);    // Define the different analog inputs for the four EMG shields that are connected to the 
 AnalogIn S1(A1);
 AnalogIn S2(A2);
 AnalogIn S3(A3);
 
-DigitalOut motor1Direction(D7);
-FastPWM motor1Velocity(D6);
-DigitalOut motor2Direction(D4);
-FastPWM motor2Velocity(D5);
+DigitalOut motor1Direction(D7);     // Digital out to control the direction of motor1
+FastPWM motor1Velocity(D6);         // FastPWM to control the velocity of motor1
+DigitalOut motor2Direction(D4);     // Digital out to control the direction of motor2
+FastPWM motor2Velocity(D5);         // FastPWM to control the velocity of motor2
 
-// Encoders 1 and 2 respectively
+// Encoder for motor1 and motor2
 QEI Encoder1(D8,D9,NC,8400);
 QEI Encoder2(D11,D10,NC,8400);
 
-Ticker measurecontrol;              // Ticker function for the measurements
+Ticker measurecontrol;      // Ticker function for the measurements
+
+Serial pc(USBTX, USBRX);    // Used to get information about the different states and choises if a laptop is attached
 
-// Make arrays for the different variables for the motors
-//AnalogIn Shields[4] = {S0, S1, S2, S3};
-//DigitalOut MotorDirections[2] = {MD0, MD1};
-//FastPWM MotorVelocities[2] = {MV0, MV1};
-//QEI Encoders[2] = {E0, E1};
-
-Serial pc(USBTX, USBRX);
-
-double PI = 3.14159265358;//97932384626433832795028841971693993;
+double PI = 3.14159265358;  // Defined pi because it is used multiple times
 volatile double timeinterval = 0.001f;      // Time interval of the Ticker function
-volatile double frequency = 17000.0;        // Set motorfrequency
+volatile double frequency = 17000.0;        // Motorfrequency of both motors
 double period_signal = 1.0/frequency;       // Convert to period of the signal
 
+/*
+ Motor calibration is done by manually moving the arms to the start position. This is the position in which the arms
+ that are connected to the motors are in line with each other. If they are in the correct position, the robot needs
+ to be reset. Then this programm will run and yendeffector and xendeffector are set to be 10.6159 and 0 which are the
+ correct physical positions on the board.
+*/
 double yendeffector = 10.6159;
 double xendeffector = 0;
-int ingedrukt = 0;
-int state_int;
-int previous_state_int;
-double motorvalue1;
-double motorvalue2;
+
+/*
+ Boolean that keeps track in which configuration the hook is
+ when 0, the hook is ready to grab a ring
+ when 1, it is ready to drob an object if it grabbed one
+*/
+bool pressed = 0;
+
+int previous_state_int; // Keeps track of the previous state to check if the state has changed
+double motorvalue1;     // Motorvalue that has to be send to motor1
+double motorvalue2;     // Motorvalue that has to be send to motor2
+
 // Define the different states in which the robot can be
 enum States {MOTORS_OFF, EMG_CALIBRATION, START_GAME,
              DEMO_MODE, OPERATING_MODE, END_GAME
@@ -64,79 +71,58 @@
 
 
 // Initialise the functions
-
+double PID_controller1(double error1);
+double PID_controller2(double error2);
+void getmeasuredposition(double & measuredposition1, double & measuredposition2);
+void getreferenceposition(double & referenceposition1, double & referenceposition2);
+void sendtomotor(double motorvalue1, double motorvalue2);
+void measureandcontrol();
 void motorsoff();
-
 void measure_data(double &f_y0, double &f_y1, double &f_y2, double &f_y3);
-
 void det_max(double y, double &max_y);
-
 void emgcalibration();
-
-void nothing()
-{
-    // Do nothing
-}
-
 void startgame() ;
-
 void demo_mode();
-
 void operating_mode();
-
+void endgame();
 void New_State();
 
-void Set_State();
-
-double PID_controller1(double error1);
-
-double PID_controller2(double error2);
-
-void getmeasuredposition(double & measuredposition1, double & measuredposition2);
-
-void getreferenceposition(double & referenceposition1, double & referenceposition2);
-
-void sendtomotor(double motorvalue1, double motorvalue2);
-
-void measureandcontrol();
-
 
 
 int main()
 {
-    ledr = 1;
+    ledr = 1;   // Set all LEDs to off
     ledg = 1;
     ledb = 1;
-    pc.baud(115200);
+    pc.baud(115200);    // Define bitrate for the communication
     pc.printf("Starting...\r\n\r\n");
 
-        motor1Velocity.period(period_signal);   // Set the period of the PWMfunction
-        motor2Velocity.period(period_signal);   // Set the period of the PWMfunction
+    motor1Velocity.period(period_signal);   // Set the period of the PWMfunction
+    motor2Velocity.period(period_signal);   // Set the period of the PWMfunction
 
     previous_state_int = (int)MyState;  // Save previous state to compare with current state and possibly execute New_State()
     // in the while loop
 
-    New_State();                            // Execute the functions belonging to the current state
+    New_State();    // For initialising the state of the robot run New_State() once
 
     while(true) {
-        if ( (previous_state_int - (int)MyState) != 0 ) {   // If current state is not previous state execute New_State()
-            New_State();
+        if ( (previous_state_int - (int)MyState) != 0 ) {
+            New_State();    // If current state is not previous state run New_State()
         }
     }
 }
 
 
-
+// Parallel PID controller to calculate the error for motor1
 double PID_controller1(double error1)
 {
-    // Define errors for motor 1 and 2
-    static double error_integral1 = 0;
-    static double error_prev1 = error1;
+    static double error_integral1 = 0;  // Keep track of the integral error within this function
+    static double error_prev1 = error1; // Safe previous error to use for the derivative part
 
-    // Low-pass filter
+    // Low-pass filter to reduce noise from the derivative part
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
 
-    // PID variables: we assume them to be the same for both motors
+    // PID variables: we assume them to be the same for both motors, found by trial and error
     double Kp = 63;
     double Ki = 3.64;
     double Kd = 5;
@@ -159,16 +145,16 @@
 }
 
 
+// Parallel PID controller to calculate the error for motor2
 double PID_controller2(double error2)
 {
-    // Define errors for motor 1 and 2
-    static double error_integral2 = 0;
-    static double error_prev2 = error2;
+    static double error_integral2 = 0;  // Keep track of the integral error within this function
+    static double error_prev2 = error2; // Safe previous error to use for the derivative part
 
-    // Low-pass filter
+    // Low-pass filter to reduce noise from the derivative part
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
 
-    // PID variables: we assume them to be the same for both motors
+    // PID variables: we assume them to be the same for both motors, found by trial and error
     double Kp = 63;
     double Ki = 3.64;
     double Kd = 5;
@@ -191,53 +177,49 @@
 }
 
 
-//get the measured position
+// Calculate the current configuration of both motors
 void getmeasuredposition(double & measuredposition1, double & measuredposition2)
 {
     // Obtain the counts of motors 1 and 2 from the encoder
-    int countsmotor1;
-    int countsmotor2;
-    countsmotor1 = Encoder1.getPulses();
-    countsmotor2 = Encoder2.getPulses();
+    int countsmotor1 = Encoder1.getPulses();
+    int countsmotor2 = Encoder2.getPulses();
 
     // Obtain the measured position for motor 1 and 2
+    // Devide by 8400 because 8400 counts is one rotation and times 2 to calculate the measuredposition as a fraction of the rotation
     measuredposition1 = ((double)countsmotor1) / 8400.0f * 2.0f;
     measuredposition2 = ((double)countsmotor2) / 8400.0f * 2.0f;
 }
 
 
-//get the reference of the
+// Calculates the desired configuration of both motors by using the inverse kinematics
 void getreferenceposition(double & referenceposition1, double & referenceposition2)
 {
     //Measurements of the arm
-    double L0=1.95;
-    double L1=15;
-    double L2=20;
+    double L0=1.95;     // Distance from both motors to (0,0), (0,0) is between motors 1 and 2
+    double L1=15;       // Length of the arms attached to motor 1 and 2
+    double L2=20;       // Length of the arms attached to the end-effector
 
-    //Inverse kinematics: given the end position, what are the desired motor angles of 1 and 2
+    //Inverse kinematics: given the end position, what are the desired motor angles of 1 and 2 (explained in the report)
     double desiredmotorangle1, desiredmotorangle2;
     desiredmotorangle1 = (atan2(yendeffector,(L0+xendeffector))*180.0/PI + acos((pow(L1,2)+pow(L0+xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0+xendeffector,2)+pow(yendeffector,2))))*180.0/PI)-180.0;
     desiredmotorangle2 = (atan2(yendeffector,(L0-xendeffector))*180.0/PI + acos((pow(L1,2)+pow(L0-xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0-xendeffector,2)+pow(yendeffector,2))))*180.0/PI)-180.0;
 
     //Convert motor angles to counts
-    double desiredmotorrounds1;
-    double desiredmotorrounds2;
+    double desiredmotorrounds1, desiredmotorrounds2;
     desiredmotorrounds1 = (desiredmotorangle1)/360.0;
     desiredmotorrounds2 = (desiredmotorangle2)/360.0;
 
-    //Assign this to new variables because otherwise it doesn't work
+    // Assign this to new variables because otherwise it doesn't work
     referenceposition1 = desiredmotorrounds1;
     referenceposition2 = desiredmotorrounds2;
 }
 
 
-//send value to motor
-// IT WAS "void sendtomotor(float & motorvalue1, float & motorvalue2)" BUT I REMOVED THE REFERENCE, BECAUSE I THOUGHT IT WAS NOT NEEDED
+// Send the absolute motorvalues as velocity to the motors and set the motor directions by checking if the motorvalues are positive
 void sendtomotor(double motorvalue1, double motorvalue2)
 {
     // Define the absolute motor values
-    double absolutemotorvalue1;
-    double absolutemotorvalue2;
+    double absolutemotorvalue1, absolutemotorvalue2;
     absolutemotorvalue1 = fabs(motorvalue1);
     absolutemotorvalue2 = fabs(motorvalue2);
 
@@ -254,7 +236,8 @@
     motor2Direction = (motorvalue2 > 0.0f);
 }
 
-// function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
+
+// function to call different functions and control the motors
 void measureandcontrol()
 {
     // Get the reference positions of motor 1 and 2
@@ -266,25 +249,20 @@
     getmeasuredposition(measured1, measured2);
 
     // Calculate the motor values
-    //double motorvalue1, motorvalue2;
     motorvalue1 = PID_controller1(reference1 - measured1);
     motorvalue2 = PID_controller2(reference2 - measured2);
     sendtomotor(motorvalue1, motorvalue2);
 }
 
 
-void motorsoff()
-{
-    // Function to turn the motors off. First state that the robot has. Robot will stay in this state untill button SW2 is pressed.
-    // Robot will not return to this state anymore unless the user sets it back to this state with the keyboard input.
+// Function that is called once in the MOTORS_OFF state, motorvelocities are zero, will run the while loop until button1 is pressed.
+void motorsoff() {
     sendtomotor(0.0,0.0);           // Set motor velocity to 0
 
-    state_int = 10;
-
     bool whileloop_boolean = true;  // Boolean for the while loop
 
     while (whileloop_boolean) {
-        if (but3.read() == 0) {                     // If button SW2 is pressed:
+        if (button1.read() == 0) {                  // If button1 is pressed:
             MyState = (States)((int)MyState+1);     // set MyState to EMG_CALIBRATION and exit the while loop
             whileloop_boolean = false;              // by making whileloop_boolean equal to false
             wait(0.5f);
@@ -292,9 +270,14 @@
     }
 }
 
+/*
+ Uses a high-pass filter, rectifier and a low-pass filter on all EMG signals and then normalises the signal
+ by deviding by the maximum value that was measured during the EMG_CALIBRATION state. This maximum value
+ is determined within this function if the state is EMG_CALIBRATION
+*/
 void measure_data(double &f_y0, double &f_y1, double &f_y2, double &f_y3)
 {
-    // High pass
+    // High-pass
     double hb0 = 0.9169;  // Coefficients from the following formula:
     double hb1 = -1.8338; //
     double hb2 = 0.9169;  //        b0 + b1 z^-1 + b2 z^-2
@@ -302,7 +285,7 @@
     double ha1 = -1.8268; //        a0 + a1 z^-1 + a2 z^-2
     double ha2 = 0.8407;  //
 
-    // Low pass
+    // Low-pass
     double lb0 = 0.000083621;  // Coefficients from the following formula:
     double lb1 = 0.0006724; //
     double lb2 = 0.000083621;  //        b0 + b1 z^-1 + b2 z^-2
@@ -310,41 +293,40 @@
     double la1 = -1.9740; //        a0 + a1 z^-1 + a2 z^-2
     double la2 = 0.9743;  //
 
-    static double max_y0 = 0.001;
-    static double max_y1 = 0.001;
+    static double max_y0 = 0.001;   // Maxima have to be static because they have to be changed during the
+    static double max_y1 = 0.001;   // calibration and have to be used afterwards to normalise the signal
     static double max_y2 = 0.001;
     static double max_y3 = 0.001;
 
-    static BiQuad hFilter0(hb0,hb1,hb2,ha0,ha1,ha2);  // Create 4 equal filters used for the different EMG signals
+    static BiQuad hFilter0(hb0,hb1,hb2,ha0,ha1,ha2);  // Create 4 equal high-pass filters used for the different EMG signals
     static BiQuad hFilter1(hb0,hb1,hb2,ha0,ha1,ha2);
     static BiQuad hFilter2(hb0,hb1,hb2,ha0,ha1,ha2);
     static BiQuad hFilter3(hb0,hb1,hb2,ha0,ha1,ha2);
 
-    static BiQuad lFilter0(lb0,lb1,lb2,la0,la1,la2);  // Create 4 equal filters used for the different EMG signals
+    static BiQuad lFilter0(lb0,lb1,lb2,la0,la1,la2);  // Create 4 equal low-pass filters used for the different EMG signals
     static BiQuad lFilter1(lb0,lb1,lb2,la0,la1,la2);
     static BiQuad lFilter2(lb0,lb1,lb2,la0,la1,la2);
     static BiQuad lFilter3(lb0,lb1,lb2,la0,la1,la2);
 
-    f_y0 = hFilter0.step(S0);     // Apply filters on the different EMG signals
+    f_y0 = hFilter0.step(S0);     // Apply high-pass filters on the different EMG signals
     f_y1 = hFilter1.step(S1);
     f_y2 = hFilter2.step(S2);
     f_y3 = hFilter3.step(S3);
 
-    f_y0 = abs(f_y0);
+    f_y0 = abs(f_y0);   // Apply rectifier on the different EMG signals
     f_y1 = abs(f_y1);
     f_y2 = abs(f_y2);
     f_y3 = abs(f_y3);
 
-    f_y0 = lFilter0.step(f_y0);
+    f_y0 = lFilter0.step(f_y0); // Apply low-pass filters on the different EMG signals
     f_y1 = lFilter1.step(f_y1);
     f_y2 = lFilter2.step(f_y2);
     f_y3 = lFilter3.step(f_y3);
 
 
     if (MyState == EMG_CALIBRATION) {
-
-        det_max(f_y0, max_y0);       // Determine the maximum RMS value of the EMG signals during the EMG_CALIBRATION state
-        det_max(f_y1, max_y1);
+        det_max(f_y0, max_y0);      // Determine the maximum value of the EMG signals during the EMG_CALIBRATION state
+        det_max(f_y1, max_y1);      // If a new maximum is found, the max_y_ value is adjusted
         det_max(f_y2, max_y2);
         det_max(f_y3, max_y3);
 
@@ -357,65 +339,74 @@
 
 }
 
+
+// Function to determine if y is larger than max_y
 void det_max(double y, double &max_y)
 {
-    max_y = max_y < y ? y : max_y; // if max_rms is smaller than rms, set rms as new max_rms, otherwise keep max_rms
+    max_y = max_y < y ? y : max_y; // if max_y is smaller than y, set y as new max_y, otherwise keep max_y
 }
 
+
+// Function that is attached to the measurecontrol ticker when the state is EMG_CALIBRATION, the ticker will run this function for a specific amount of time
 void emgcalibration()
 {
-    double y0, y1, y2, y3;           // RMS values of the different EMG signals
+    double y0, y1, y2, y3;           // variables to safe the output from measure_data
 
-    measure_data(y0, y1, y2, y3);   // Calculate RMS values
+    measure_data(y0, y1, y2, y3);   // Calculate y values
 
-    double duration = 20.0;                 // Duration of the emgcalibration function, in this case 10 seconds
+    double duration = 20.0;                 // Duration that the emgcalibration() has to run
     int rounds = (duration / timeinterval); // Determine the amount of times this function has to run to run for the duration time
-    // rounds is an integer so the value of duration / timeinterval is floored
+                                            // Rounds is an integer so the value of duration / timeinterval is floored
 
-    static int counter = 0;             // Counter which keeps track of the amount of times the function has executed
+    static int counter = 0;     // Counter which keeps track of the amount of times the function has executed
     if (counter >= rounds) {
-        MyState = START_GAME;    // If counter is larger than rounds, change MyState to the next state
-        ledb = 1;
-        measurecontrol.detach();
+        MyState = START_GAME;       // If counter is larger than rounds, change MyState to the START_GAME state
+        ledb = 1;                   // Turn the blue LED off
+        measurecontrol.detach();    // detach emgcalibration() from the ticker function
     } else {
         counter++;  // Else increase counter by 1
     }
     
-    int duration_led = 0.1 / timeinterval;
-    if (counter % duration_led == 0) {
+    int rounds_led = 0.1f / timeinterval;  // Amount of rounds that the blue LED has to be on and off
+    if (counter % rounds_led == 0) {
         ledb = !ledb;
     }
 }
 
+
+// Function that is executed when the state is changed to START_GAME, will run a while loop until
+// button1 or 2 are pressed to choose the game mode
 void startgame()
 {
+    // Print messages with information for the user
     pc.printf("Please choose which game you would like to start:\r\n");
 
-    pc.printf("- Press button SW2 to start the demo mode\r\n     Demo mode is a mode in which the different movements of the robot are shown.\r\n");
+    pc.printf("- Press button1 to start the demo mode\r\n     Demo mode is a mode in which the different movements of the robot are shown.\r\n");
     pc.printf("     It will move in straight lines to show that the robot meets the requirements.\r\n");
     pc.printf("     It will also show how it is able to grab and lift objects which are on the board.\r\n\r\n");
 
-    pc.printf("- Press button SW3 to start the operating mode\r\n     The operating mode is a mode in which you can control the robot by flexing the muscles to which the electrodes are attached.\r\n");
+    pc.printf("- Press button2 to start the operating mode\r\n     The operating mode is a mode in which you can control the robot by flexing the muscles to which the electrodes are attached.\r\n");
     pc.printf("     You will be able to move the arm and use the gripper to try and grab and lift objects from the board to the container.\r\n\r\n");
 
     while (MyState == START_GAME) {
-
-        if (but3.read() == 0) {
-            MyState = (States)((int)MyState+1);
+        if (button1.read() == 0) {
+            MyState = DEMO_MODE;        // Press button1 to choose DEMO_MODE
             wait(0.5f);
-        } else if (but4.read() == 0) {
-            MyState = (States)((int)MyState+2);
+        } else if (button2.read() == 0) {
+            MyState = OPERATING_MODE;   // Press button2 to choose OPERATING_MODE
             wait(0.5f);
         }
-
     }
 }
 
 
+// Function that is executed when the state is changed to DEMO_MODE, will move the end-effector
+// to 5 different positions to show it can move in straight lines and then go back to the START_GAME state
 void demo_mode()
 {
-
-    // 5 pre determined positions of the end effector to show it can move in straight lines
+    // CURRENTLY STILL NOT WORKING, ARMS WILL MOVE TO SINGULARITY, WATCH OUT WITH THIS MODE
+    
+    // 5 pre determined positions of the end-effector to show it can move in straight lines
     xendeffector=-5;
     yendeffector=10.6159;
     wait(0.5);
@@ -440,49 +431,55 @@
     MyState = START_GAME;     // Go back to START_GAME mode
 }
 
-
+/*
+ Function that is attached to the measurecontrol ticker when the state is OPERATING_MODE
+ Turns the green LED on and off, calls measuredata(), changes the configuration of the hook
+ when button2 is pressed, sets a new position for the end-effector and goes to the state
+ END_GAME when button1 is pressed.
+*/
 void operating_mode()
 {
-    // green turns on and off while running this function
-    static int counter = 0;
-    int duration_led = 0.1 / timeinterval;
-    
-    if (counter % duration_led == 0) {
+    // Turn green LED on or off
+    static int counter = 0;     // Keep track of the amount of times the function has executed
+    int rounds_led = 0.1 / timeinterval;    // Amount of rounds that the green LED has to be on and off
+    if (counter % rounds_led == 0) {
         ledg = !ledg;
         counter = 0;
     }
     counter++;
     
-    
     double y0,y1,y2,y3;
-    measure_data(y0,y1,y2,y3);
+    measure_data(y0,y1,y2,y3);  // Calculate y values
     
     //servo besturing
-    
-    if(but4.read() == 0 && ingedrukt == 0) {
-        for(int i=0; i<100; i++) {
-            myservo = i/100.0;
-        }
-        ingedrukt = 1;
+    if(button2.read() == 0 && pressed == 0) {
+        // if button2 is pressed, set y0, y1, y2 and y3 to zero, change pressed and change
+        // the servo configuration one step at a time to reduce the speed of the rotation
         y0 = 0;
         y1 = 0;
         y2 = 0;
         y3 = 0;
-    }
-    else if(but4.read() == 0 && ingedrukt == 1) {
-        for(int i=100; i>0; i--) {
+        pressed = 1;
+        for(int i=0; i<100; i++) {
             myservo = i/100.0;
         }
+    }
+    else if(button2.read() == 0 && pressed == 1) {
+        // if button2 is pressed, set y0, y1, y2 and y3 to zero, change pressed and change
+        // the servo configuration one step at a time to reduce the speed of the rotation
         y0 = 0;
         y1 = 0;
         y2 = 0;
         y3 = 0;
-        ingedrukt = 0;
+        pressed = 0;
+        for(int i=100; i>0; i--) {
+            myservo = i/100.0;
+        }
     }
     
-    
-    double threshold = 0.4;     // When the analysed signal is above this threshold, the position of the end effector is changed
-    double dr = 0.01;            // The change in position with each step
+    // The threshold has been set to 0.4 by trial and error
+    double threshold = 0.4;     // When the analysed signal is above this threshold, the position of the end-effector is changed
+    double dr = 0.01;           // The change in position with each execution
     
     if(y0 > threshold && xendeffector < 16) {
         xendeffector=xendeffector+dr;
@@ -490,6 +487,7 @@
     else if(y1 > threshold && xendeffector > -16) {
         xendeffector=xendeffector-dr;
     }
+    // The threshold was adjusted for the y-position because this worked better, this maybe had to be done because of a broken EMG cable
     if(y2 > threshold+0.2 && yendeffector < 28) {
         yendeffector=yendeffector+dr;
     }
@@ -497,43 +495,49 @@
         yendeffector=yendeffector-dr;
     }
 
-    
-    if (but3.read() == 0) {
+    // When button1 is pressed, run while loop until button is released again to prevent bouncing
+    if (button1.read() == 0) {
         bool buttonss = true;
         while (buttonss) {
-            if (but3.read() == 1) {
+            if (button1.read() == 1) {
                 buttonss = false;
             }
         }
-        pc.printf("The game has ended, will move the end effector to (0,0), put the motors off and will now return to the state START_GAME");
-        MyState = END_GAME;
-        measurecontrol.detach();
+        pc.printf("The game has ended, will move the end-effector to (0,0), put the motors off and will return to the state START_GAME");
+        MyState = END_GAME;         // Set MyState to END_GAME
+        measurecontrol.detach();    // Detach the operating_mode() function from measurecontrol
     }
 
-    measureandcontrol();
+    measureandcontrol();    // Call measureandcontrol() to adjust the motors
 }
 
+
+// Function that is executed once when state is changed to END_GAME, returns the end-effector to
+// the start position and detaches the measureandcontrol function from the measurecontrol ticker
 void endgame()
 {
     measurecontrol.attach(measureandcontrol,timeinterval);
     
     double tempx = xendeffector;
     double tempy = yendeffector-10.6159;
-    double steps = 20;
+    double steps = 20;  // Change the position of the end-effector in a certain amount of steps to the start-position
+    // This is done in steps to reduce abrupt movement of the end-effector
     
     for (int i = 0; i < steps; i++) {
         xendeffector = xendeffector - tempx/steps;
         yendeffector = yendeffector - tempy/steps;
         wait(0.1);
     }
-
     measurecontrol.detach();    
-    MyState = START_GAME;
+    MyState = START_GAME;       // Return the state to START_GAME so a new game mode can be chosen again
 }
 
+
+// Function that is called by the while loop in the main function if the current state is changed
+// Attaches or executes the functions belonging to the current state
 void New_State()
 {
-    previous_state_int = (int)MyState;                  // Change previous state to current state
+    previous_state_int = (int)MyState;      // Change previous state to current state
 
     switch (MyState) {
         case MOTORS_OFF :
@@ -546,7 +550,7 @@
         case EMG_CALIBRATION :
             pc.printf("\r\nState: EMG Calibration\r\n");
             pc.printf("Emg calibration mode will run for 20 seconds. Try to flex the muscles to which the electrodes are attached as hard as possible during this time.\r\n");
-            measurecontrol.attach(emgcalibration,timeinterval);
+            measurecontrol.attach(emgcalibration,timeinterval); // Attach emgcalibration() to the measurecontrol ticker
             break;
 
         case START_GAME :
@@ -559,12 +563,12 @@
 
         case DEMO_MODE :
             pc.printf("\r\nState: Demo mode\r\n");
-            measurecontrol.attach(measureandcontrol,timeinterval);
+            measurecontrol.attach(measureandcontrol,timeinterval); // Attach measureandcontrol() to the measurecontrol ticker
             ledr = 1;       // red led is on when you enter START_GAME state
             ledb = 0;       // blue led is on when you enter START_GAME state
             ledg = 0;       // green led is on when you enter START_GAME state
             demo_mode();
-            measurecontrol.detach();
+            measurecontrol.detach();    // Detach measureandcontrol() grom the measurecontrol ticker
             break;
 
         case OPERATING_MODE :
@@ -572,7 +576,7 @@
             ledr = 1;       // red led is off when you enter OPERATING_MODE state
             ledb = 1;       // blue led is off when you enter OPERATING_MODE state
             ledg = 0;       // green led is on when you enter OPERATING_MODE state
-            measurecontrol.attach(operating_mode,timeinterval);
+            measurecontrol.attach(operating_mode,timeinterval); // Attach operating_mode() to the measurecontrol ticker
             break;
 
         case END_GAME :
@@ -584,44 +588,14 @@
             break;
 
         default :
-            pc.printf("\r\nDefault state: Motors are turned off\r\n");
-             
+            pc.printf("\r\nSTATE IS CHANGED TO A NONEXISTING STATE!\r\n");
+            pc.printf("\r\nMotors are turned off\r\n");
             sendtomotor(0.0,0.0);
-            break;
+            ledg = 1;
+            ledb = 1;
+            while (true) {
+                ledr = !ledr;
+                wait(0.1f);
+            }
     }
 }
-
-void Set_State()
-{
-    xendeffector=0.0;
-    yendeffector=10.6159;
-    wait(0.3f);
-    sendtomotor(0.0,0.0);                       // Stop the motors
-        // Stop the ticker function from running
-
-    pc.printf("\r\nPress number:     | To go to state:");
-    pc.printf("\r\n              (0) | MOTORS_OFF: Set motorspeed just in case to 0 and wait till button SW2 is pressed");
-    pc.printf("\r\n              (1) | EMG_CALIBRATION: Calibrate the maximum of the emg signals and normalise the emg signals with these maxima");
-    pc.printf("\r\n              (2) | START_GAME: Determine by keyboard input if you want to go to the demo or operating mode");
-    pc.printf("\r\n              (3) | DEMO_MODE: The demo mode will show the different motions that the robot can make");
-    pc.printf("\r\n              (4) | OPERATING_MODE: Move the arms and gripper of the arm by flexing your muscles");
-    pc.printf("\r\n              (5) | END_GAME: End effector returns to (0,0) and the motors are turned off, returns to START_GAME mode afterwards");
-
-    wait(0.5f);
-
-    char a = '0';
-    char b = '5';
-    bool boolean = true;
-
-    while (boolean) {
-        char c = pc.getc();
-
-        if (c >= a && c <= b) {
-            MyState = (States)(c-'0');
-            boolean = false;
-
-        } else {
-            pc.printf("\r\nPlease enter a number between 0 and 5\r\n");
-        }
-    }
-}