Diff: main.cpp

Revision:

23:ff73ee119244

Parent:

22:cce4dc5738af

Child:

24:e87e4fcf6226

--- a/main.cpp	Wed Oct 30 11:34:47 2019 +0000
+++ b/main.cpp	Thu Oct 31 09:48:53 2019 +0000
@@ -1,6 +1,5 @@
-// MOTOR_CONTROL FUNCTION HAS TO BE ADJUSTED TO TWO MOTORS
-// reference velocity has to be fixed? idk? --> wait for file from bram en paul
-// Coefficients for the filters have to be adjusted --> Is 1 filter enough?
+// Ticker function.attach(nothing,1000) good solution??????
+// Operating mode might not go to next state when SW2 is pressed
 
 #include "mbed.h"
 #include "HIDScope.h"
@@ -9,66 +8,66 @@
 #include "FastPWM.h"
 #include "QEI.h"
 
-#include <cmath>
-#include <deque>
+#include <cmath>    // Included to use different math operations
 
-#include "Motor_Control.h"
+#include "Servo.h"  // Included to control the servo motor 
 
-DigitalIn button1(D13);     // Button1 input to go to next state
-InterruptIn button2(SW2);   // Button2 input to activate failuremode()
-InterruptIn button3(SW3);   // Button3 input to go to a specific state
-DigitalOut ledg(LED_GREEN); // to test stuff
-DigitalOut ledr(LED_RED);   // Red LED output to show
+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
 
 AnalogIn S0(A0), S1(A1), S2(A2),S3(A3);    // Input EMG Shield 0,1,2,3
 DigitalOut  MD0(D7), MD1(D4); // MotorDirection of motors 0,1,2
-FastPWM MV0(D6), MV1(D5), MV2(D12); // MotorVelocities of motors 0,1,2
-QEI E0(D8,D9,NC,8400), E1(D10,D11,NC,8400); // Encoders of motors 0,1,2
+FastPWM MV0(D6), MV1(D5); // MotorVelocities of motors 0,1,2
+QEI E0(D8,D9,NC,8400), E1(D11,D10,NC,8400); // Encoders of motors 0,1,2
 
-Ticker measurecontrol;              // Ticker function for motor in- and output
+Ticker measurecontrol;              // Ticker function for the measurements
 
 // Make arrays for the different variables for the motors
 AnalogIn Shields[4] = {S0, S1, S2, S3};
 DigitalOut MotorDirections[2] = {MD0, MD1};
-FastPWM MotorVelocities[3] = {MV0, MV1, MV2};
+FastPWM MotorVelocities[2] = {MV0, MV1};
 QEI Encoders[2] = {E0, E1};
 
 Serial pc(USBTX, USBRX);
 
-float PI = 3.1415926f;//535897932384626433832795028841971693993;
-float timeinterval = 0.001f;        // Time interval of the Ticker function
-bool whileloop = true;              // Statement to keep the whileloop in the main function running
-// While loop has to stop running when failuremode is activated
+double PI = 3.14159265358;//97932384626433832795028841971693993;
+volatile double timeinterval = 0.001f;      // Time interval of the Ticker function
+volatile double frequency = 17000.0;        // Set motorfrequency
+double period_signal = 1.0/frequency;       // Convert to period of the signal
+
+double yendeffector = 10.6159;
+double xendeffector = 0;
+int ingedrukt = 0;
+int state_int;
 
 // Define the different states in which the robot can be
-enum States {MOTORS_OFF, EMG_CALIBRATION, MOTOR_CALIBRATION, START_GAME,
-             DEMO_MODE, OPERATING_MODE, MOVE_GRIPPER, END_GAME, FAILURE_MODE
+enum States {MOTORS_OFF, EMG_CALIBRATION, START_GAME,
+             DEMO_MODE, OPERATING_MODE, END_GAME
             };
 
-
 // Default state is the state in which the motors are turned off
 States MyState = MOTORS_OFF;
 
+
 // Initialise the functions
 
 void motorsoff();
 
-void measure_data(double &rms_0, double &rms_1, double &rms_2, double &rms_3);
+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();
 
-double P_controller(double error);
-
 void nothing()
 {
     // Do nothing
 }
 
-void get_input(char c);
+void startgame() ;
 
-void startgame() ;
+void demo_mode();
 
 void operating_mode();
 
@@ -76,66 +75,209 @@
 
 void Set_State();
 
-void failuremode();
+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()
 {
     pc.baud(115200);
-    ledr = 1;
-    ledg = 1;
+    pc.printf("Starting...\r\n\r\n");
 
-    pc.printf("Starting...\r\n\r\n");
-    double frequency = 17000.0;            // Set motorfrequency
-    double period_signal = 1.0/frequency;  // Convert to period of the signal
-    pc.baud(115200);
-
-    button2.fall(failuremode);              // Function is always activated when the button is pressed
-    button3.fall(Set_State);                // Function is always activated when the button is pressed
-
-    for (int i = 0; i < 3; i++) {
+    for (int i = 0; i < 2; i++) {
         MotorVelocities[i].period(period_signal);   // Set the period of the PWMfunction
     }
 
-    measurecontrol.attach(measureandcontrol, timeinterval); // Ticker function to measure motor input and control the motors
+    measurecontrol.attach(nothing, timeinterval); // Ticker function to measure motor input and control the motors
 
     int 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
 
-    while(whileloop) {
+    while(true) {
         if ( (previous_state_int - (int)MyState) != 0 ) {   // If current state is not previous state execute New_State()
             New_State();
         }
-        previous_state_int = (int)MyState;                  // Change previous state to current state
+
+        previous_state_int = (int)MyState-state_int;                  // Change previous state to current state
+        state_int = 0;
     }
-    // While has stopped running
-    pc.printf("Programm stops running\r\n");    // So show that the programm is quiting
-    sendtomotor(0.0);                          // Set the motor velocity to 0
-    measurecontrol.attach(nothing,10000);       // Attach empty function to Ticker (?? Appropriate solution ??)
+}
+
+
+
+double PID_controller1(double error1)
+{
+    // Define errors for motor 1 and 2
+    static double error_integral1 = 0;
+    static double error_prev1 = error1;
+
+    // Low-pass filter
+    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
+    double Kp = 65;
+    double Ki = 3.64;
+    double Kd = 5;
+
+    //Proportional part:
+    double u_k1 = Kp * error1;
+
+    // Integreal part
+    error_integral1 = error_integral1 + error1 * timeinterval;
+    double u_i1 = Ki*error_integral1;
+
+    // Derivate part
+    double error_derivative1 = (error1 - error_prev1)/timeinterval;
+    double filtered_error_derivative1 = LowPassFilter.step(error_derivative1);
+    double u_d1 = Kd * filtered_error_derivative1;
+    error_prev1 = error1;
+
+    //sum all parts and return it
+    return u_k1 + u_i1 + u_d1;
+}
+
+
+double PID_controller2(double error2)
+{
+    // Define errors for motor 1 and 2
+    static double error_integral2 = 0;
+    static double error_prev2 = error2;
+
+    // Low-pass filter
+    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
+    double Kp = 65;
+    double Ki = 3.64;
+    double Kd = 5;
+
+    //Proportional part:
+    double u_k2 = Kp * error2;
+
+    // Integreal part
+    error_integral2 = error_integral2 + error2 * timeinterval;
+    double u_i2 = Ki*error_integral2;
+
+    // Derivate part
+    double error_derivative2 = (error2 - error_prev2)/timeinterval;
+    double filtered_error_derivative2 = LowPassFilter.step(error_derivative2);
+    double u_d2 = Kd * filtered_error_derivative2;
+    error_prev2 = error2;
+
+    //sum all parts and return it
+    return u_k2 + u_i2 + u_d2;
+}
+
 
-    return 0;
+//get the measured position
+void getmeasuredposition(double & measuredposition1, double & measuredposition2)
+{
+    // Obtain the counts of motors 1 and 2 from the encoder
+    int countsmotor1;
+    int countsmotor2;
+    countsmotor1 = Encoders[0].getPulses();
+    countsmotor2 = Encoders[1].getPulses();
+
+    // Obtain the measured position for motor 1 and 2
+    measuredposition1 = ((double)countsmotor1) / 8400.0f * 2.0f;
+    measuredposition2 = ((double)countsmotor2) / 8400.0f * 2.0f;
+}
+
+
+//get the reference of the
+void getreferenceposition(double & referenceposition1, double & referenceposition2)
+{
+    //Measurements of the arm
+    double L0=1.95;
+    double L1=15;
+    double L2=20;
+
+    //Inverse kinematics: given the end position, what are the desired motor angles of 1 and 2
+    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;
+    desiredmotorrounds1 = (desiredmotorangle1)/360.0;
+    desiredmotorrounds2 = (desiredmotorangle2)/360.0;
+
+    //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
+void sendtomotor(double motorvalue1, double motorvalue2)
+{
+    // Define the absolute motor values
+    double absolutemotorvalue1;
+    double absolutemotorvalue2;
+    absolutemotorvalue1 = fabs(motorvalue1);
+    absolutemotorvalue2 = fabs(motorvalue2);
+
+    // If absolutemotorvalueocity is greater than 1, reduce to 1, otherwise remain absolutemotorvalue
+    absolutemotorvalue1 = absolutemotorvalue1 > 1.0f ? 1.0f : absolutemotorvalue1;
+    absolutemotorvalue2 = absolutemotorvalue2 > 1.0f ? 1.0f : absolutemotorvalue2;
+
+    // Send the absolutemotorvalue to the motors
+    MotorVelocities[0] = absolutemotorvalue1;
+    MotorVelocities[1] = absolutemotorvalue2;
+
+    // Determine the motor direction. Boolean output: true gives counterclockwise direction, false gives clockwise direction
+    MotorDirections[0] = (motorvalue1 > 0.0f);
+    MotorDirections[1] = (motorvalue2 > 0.0f);
+}
+
+// function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
+void measureandcontrol()
+{
+    // Get the reference positions of motor 1 and 2
+    double reference1, reference2;
+    getreferenceposition(reference1, reference2);
+
+    // Get the measured positions of motor 1 and 2
+    double measured1, measured2;
+    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 button1 is pressed.
+    // 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.
+    sendtomotor(0.0,0.0);           // Set motor velocity to 0
+
+    state_int = 10;
 
     bool whileloop_boolean = true;  // Boolean for the while loop
-    sendtomotor(0.0);              // Set motor velocity to 0
 
     while (whileloop_boolean) {
-        if (button1.read() == 0) {          // If button1 is pressed:
-            MyState = EMG_CALIBRATION;      // set MyState to EMG_CALIBRATION and exit the while loop
-            whileloop_boolean = false;      // by making whileloop_boolean equal to false
-        } else if (MyState != MOTORS_OFF) { // If the state is changed by keyboard input, exit the while loop
-            whileloop_boolean = false;
+        if (but3.read() == 0) {                     // If button SW2 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);
         }
     }
 }
@@ -158,10 +300,10 @@
     double la1 = -1.9740; //        a0 + a1 z^-1 + a2 z^-2
     double la2 = 0.9743;  //
 
-    static double max_y0 = 1;
-    static double max_y1 = 1;
-    static double max_y2 = 1;
-    static double max_y3 = 1;
+    static double max_y0 = 0.001;
+    static double max_y1 = 0.001;
+    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 hFilter1(hb0,hb1,hb2,ha0,ha1,ha2);
@@ -173,10 +315,10 @@
     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_y1 = hFilter1.step(S1);
-    f_y2 = hFilter2.step(S2);
-    f_y3 = hFilter3.step(S3);
+    f_y0 = hFilter0.step(Shields[0]);     // Apply filters on the different EMG signals
+    f_y1 = hFilter1.step(Shields[1]);
+    f_y2 = hFilter2.step(Shields[2]);
+    f_y3 = hFilter3.step(Shields[3]);
 
     f_y0 = abs(f_y0);
     f_y1 = abs(f_y1);
@@ -196,9 +338,9 @@
         det_max(f_y2, max_y2);
         det_max(f_y3, max_y3);
 
-    } else if ((int)MyState > 4) {
+    } else if ((int)MyState > 3) {
         f_y0 = f_y0/max_y0;     // Normalise the RMS value by dividing by the maximum RMS value
-        f_y1 = f_y1/max_y1;     // This is done during the states with a value higher than 4, as this is when you start the operating mode
+        f_y1 = f_y1/max_y1;     // This is done during the states with a value higher than 3, as this is when you start the operating mode
         f_y2 = f_y2/max_y2;
         f_y3 = f_y3/max_y3;
     }
@@ -222,79 +364,125 @@
 
     static int counter = 0;             // Counter which keeps track of the amount of times the function has executed
     if (counter >= rounds) {
-        MyState = MOTOR_CALIBRATION;    // If counter is larger than rounds, change MyState to MOTOR_CALIBRATION
+        MyState = (States)((int)MyState+1);    // If counter is larger than rounds, change MyState to the next state
+        measurecontrol.attach(nothing,10000);
     } else {
         counter++;  // Else increase counter by 1
     }
 
 }
 
-//P control implementation (behaves like a spring)
-double P_controller(double error)
-{
-    double Kp = 17.5;
-    //Proportional part:
-    double u_k = Kp * error;
-
-    //sum all parts and return it
-    return u_k;
-}
-/*
-void get_input(char c)
-{
-    if (c == 'd') {
-        MyState = DEMO_MODE;
-    } else if (c == 'o') {
-        MyState = OPERATING_MODE;
-    } else {
-        pc.printf("'d' or 'o' were not pressed\r\nPress 'd' to start the demo mode and press 'o' to start the operating mode\r\n");
-        get_input(pc.getc());
-    }
-}
-*/
 void startgame()
 {
     pc.printf("Please choose which game you would like to start:\r\n");
 
-    pc.printf("- Press 'd' 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 show the edges of the space that the end effector is allowed to go to and will also move in straight lines to show that the robot meets the requirements.\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("     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 'o' 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 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("     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) {
-        char c = pc.getc();
-        if (c == 'd') {
-            MyState = DEMO_MODE;
-        } else if (c == 'o') {
-            MyState = OPERATING_MODE;
+
+        if (but3.read() == 0) {
+            MyState = (States)((int)MyState+1);
+            wait(0.5f);
+        } else if (but4.read() == 0) {
+            MyState = (States)((int)MyState+2);
+            wait(0.5f);
         }
-        else {
-            pc.printf("'d' or 'o' were not pressed\r\nPress 'd' to start the demo mode and press 'o' to start the operating mode\r\n");
-        }
+
     }
 }
 
-/*
-void demo_mode() {
+
+void demo_mode()
+{
+
+    state_int = 10;
+
+    // 5 pre determined positions of the end effector to show it can move in straight lines
+    xendeffector=-5;
+    yendeffector=10.6159;
+    wait(0.5);
+
+    xendeffector=10;
+    yendeffector=25.6159;
+    wait(0.5);
 
+    xendeffector=-14;
+    yendeffector=21.6159;
+    wait(0.5);
+
+    xendeffector=10;
+    yendeffector=11.6159;
+    wait(0.5);
+
+    // Last position is the start position
+    xendeffector=0;
+    yendeffector=10.6159;
+    wait(0.5);
+
+    MyState = START_GAME;     // Go back to START_GAME mode
 }
-*/
+
 
 void operating_mode()
 {
     double y0,y1,y2,y3;
     measure_data(y0,y1,y2,y3);
-    double F_Y[4] = {y0, y1, y2, y3};
 
-    double threshold = 0.3;
-    for (int i = 0; i < 4; i++) {
-        if (F_Y[i] > threshold) {
-            //The direction and velocity of the motors are determind here
-        }
+    double threshold = 0.3;     // When the analysed signal is above this threshold, the position of the end effector is changed
+    double dr = 0.1;            // The change in position with each step
+    
+    if(y0 > threshold && xendeffector < 16) {
+        xendeffector=xendeffector+dr;
+    }
+    else if(y1 > threshold && xendeffector > -16) {
+        xendeffector=xendeffector-dr;
+    }
+    if(y2 > threshold && yendeffector < 28) {
+        yendeffector=yendeffector+dr;
+    }
+    else if(y3 > threshold && yendeffector > 8) {
+        yendeffector=yendeffector-dr;
     }
 
+    //servo besturing
+
+    if(but4.read() == 0 && ingedrukt == 0) {
+        for(int i=0; i<100; i++) {
+            myservo = i/100.0;
+
+            wait(0.01);
+        }
+        ingedrukt = 1;
+    } else if(but4.read() == 0 && ingedrukt == 1) {
+        for(int i=100; i>0; i--) {
+            myservo = i/100.0;
+
+            wait(0.01);
+        }
+        ingedrukt = 0;
+    }
+    if (but3.read() == 0) {
+        MyState = (States)((int)MyState+1);
+        wait(0.5f);
+    }
+    measureandcontrol();
+}
+
+void endgame()
+{
+    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");
+    xendeffector=0.0;
+    yendeffector=10.6159;
+    wait(0.3f);
+    sendtomotor(0.0f,0.0f);
+    wait(0.1f);
+    measurecontrol.attach(nothing,10000);
+    MyState = START_GAME;
 }
 
 void New_State()
@@ -311,10 +499,6 @@
             measurecontrol.attach(emgcalibration,timeinterval);
             break;
 
-        case MOTOR_CALIBRATION :
-            pc.printf("\r\nState: Motor Calibration\r\n");
-            break;
-
         case START_GAME :
             pc.printf("\r\nState: Start game\r\n");
             startgame();
@@ -322,7 +506,8 @@
 
         case DEMO_MODE :
             pc.printf("\r\nState: Demo mode\r\n");
-            //demo_mode();
+            measurecontrol.attach(measureandcontrol,timeinterval);
+            demo_mode();
             break;
 
         case OPERATING_MODE :
@@ -330,69 +515,50 @@
             measurecontrol.attach(operating_mode,timeinterval);
             break;
 
-        case MOVE_GRIPPER :
-            pc.printf("\r\nState: Move the gripper\r\n");
-            break;
-
         case END_GAME :
             pc.printf("\r\nState: End of the game\r\n");
-            break;
-
-        case FAILURE_MODE :
-            pc.printf("\r\nFAILURE MODE!!\r\n");        // Let the user know it is in failure mode
-            ledr = 0;                               // Turn red led on to show that failure mode is active
-            whileloop = false;
+            endgame();
             break;
 
         default :
             pc.printf("\r\nDefault state: Motors are turned off\r\n");
             measurecontrol.attach(nothing,10000);
-            sendtomotor(0.0);
+            sendtomotor(0.0,0.0);
             break;
     }
 }
 
 void Set_State()
 {
-    if (MyState == FAILURE_MODE) {
-        pc.printf("\r\nNot possible to set state because robot is in FAILURE_MODE. Please restart robot.\r\n");
-    } else {
-        sendtomotor(0.0);                       // Stop the motors
-        measurecontrol.attach(nothing,10000);   // Stop the ticker function from running
+    xendeffector=0.0;
+    yendeffector=10.6159;
+    wait(0.3f);
+    sendtomotor(0.0,0.0);                       // Stop the motors
+    measurecontrol.attach(nothing,10000);   // 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 button1 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) | MOTOR_CALIBRATION: Calibrate the motors to determine the (0,0) position.");
-        pc.printf("\r\n              (3) | START_GAME: Determine by keyboard input if you want to go to the demo or operating mode");
-        pc.printf("\r\n              (4) | DEMO_MODE: The demo mode will show the different motions that the robot can make");
-        pc.printf("\r\n              (5) | OPERATING_MODE: Move the arms and gripper of the arm by flexing your muscles");
-        pc.printf("\r\n              (6) | MOVE_GRIPPER: Control the movement of the gripper");
-        pc.printf("\r\n              (7) | END_GAME: End effector returns to (0,0) and the motors are turned off, returns to START_GAME mode afterwards");
-        pc.printf("\r\n              (8) | FAILURE_MODE: Everything is stopped and the whole programm stops running.\r\n");
+    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.5);
+    wait(0.5f);
 
-        char a = '0';
-        char b = '8';
-        bool boolean = true;
+    char a = '0';
+    char b = '5';
+    bool boolean = true;
 
-        while (boolean) {
-            char c = pc.getc();
+    while (boolean) {
+        char c = pc.getc();
 
-            if (c >= a && c <= b) {
-                MyState = (States)(c-'0');
-                boolean = false;
+        if (c >= a && c <= b) {
+            MyState = (States)(c-'0');
+            boolean = false;
 
-            } else {
-                pc.printf("\r\nPlease enter a number between 0 and 8\r\n");
-            }
+        } else {
+            pc.printf("\r\nPlease enter a number between 0 and 5\r\n");
         }
     }
 }
-
-void failuremode()
-{
-    MyState = FAILURE_MODE; // failurmode() is activated so set MyState to FAILURE_MODE
-    New_State();            // Execute actions coupled to FAILURE_MODE
-}
\ No newline at end of file