Diff: main.cpp

Revision:

21:8798f776f778

Parent:

16:ac4e3730f61f

diff -r ac4e3730f61f -r 8798f776f778 main.cpp
--- a/main.cpp	Fri Nov 02 08:05:30 2018 +0000
+++ b/main.cpp	Tue Nov 06 09:15:34 2018 +0000
@@ -12,85 +12,88 @@
 DigitalOut led_red(LED_RED);
 DigitalOut led_green(LED_GREEN);
 DigitalOut led_blue(LED_BLUE);
+
 // Buttons
-DigitalIn button_clbrt_home(SW2);
-DigitalIn button_Demo(D2);
-DigitalIn button_Emg(D3);
-DigitalIn buttonx(D2); //x EMG replacement 
-DigitalIn buttony(D3); //y EMG replacement
-DigitalIn buttondirx(D2); //x direction change 
-DigitalIn buttondiry(D3); //y direction change
-DigitalIn Fail_button(SW3);
+DigitalIn button_clbrt_home(SW2);   // button to switch to calibrating or homing state
+DigitalIn button_Demo(D2);          // button to switch to demo state
+DigitalIn button_Emg(D3);           // button to switch to move_with_EMG state
+DigitalIn buttonx(D2);              // x EMG replacement 
+DigitalIn buttony(D3);              // y EMG replacement
+DigitalIn buttondirx(D2);           // x direction change 
+DigitalIn buttondiry(D3);           // y direction change
+DigitalIn Fail_button(SW3);         // button to switch to fail state
+
 // Modserial
 MODSERIAL pc(USBTX, USBRX);
+
 // Encoders
 QEI Encoder1(D11, D10, NC, 4200) ;  // Encoder motor 1, (pin 1A, pin 1B, index pin(not used), counts/rev)
 QEI Encoder2(D9, D8, NC, 4200) ;    // Encoder motor 2, (pin 2A, pin 2B, index pin (not used), counts/rev)
+
 // Motors (direction and PWM)
-DigitalOut directionM1(D4);
+DigitalOut directionM1(D4);         
 DigitalOut directionM2(D7);
 FastPWM motor1_pwm(D5);
 FastPWM motor2_pwm(D6);
+
 // EMG input en start value of filtered EMG
-AnalogIn    emg1_raw( A0 );
+AnalogIn    emg1_raw( A0 );         // Raw EMG input
 AnalogIn    emg2_raw( A1 );
-AnalogIn potmeter1(PTC11);
+AnalogIn potmeter1(PTC11);          // Input of two potmeters
 AnalogIn potmeter2(PTC10);
 
 // Declare timers and Tickers
-Timer       timer;                        // Timer for counting time in this state
+Timer       timer;                    // Timer for counting time in this state
 //Ticker      WriteValues;            // Ticker to show values of velocity to screen
 Ticker      StateMachine;
 //Ticker      sample_EMGtoHIDscope;   // Ticker to send the EMG signals to screen
 //HIDScope    scope(4);               //Number of channels which needs to be send to the HIDScope
-//Ticker      sample;          // Ticker for reading out EMG
+//Ticker      sample;                 // Ticker for reading out EMG
 
 // Set up ProcessStateMachine
-enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_KNOPJES, MOVE_W_DEMO, FAILURE_MODE};
-states currentState = WAITING;
-bool stateChanged = true;
+enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_BUTTONS, MOVE_W_DEMO, FAILURE_MODE};
+states currentState = WAITING;      // Start in waiting state
+bool stateChanged = true;           
 
 float threshold_EMG = 0.25;         // Threshold on 25 percent of the maximum EMG
 
 // Global variables
-char c;
-//const float fs = 1/1024;
-int counts1; 
-int counts2;
-float theta1;
-float theta2;
-float vel_1;
-float vel_2;
-float theta1_prev = 0.0;
-float theta2_prev = 0.0;
-const float pi = 3.14159265359;
-volatile double  error1;
+int counts1;                        // Counts of encoder 1
+int counts2;                        // Counts of encoder 2
+float theta1;                       // Angle of motor 1 (rad)
+float theta2;                       // Angle of motor 2 (rad)
+float vel_1;                        // Velocity of motor 1
+float vel_2;                        // Velocity of motor 2
+float theta1_prev = 0.0;            // Previous angles set to zero
+float theta2_prev = 0.0;             
+const float pi = 3.14159265359;     // Define pi
+volatile double  error1;           
 volatile double  error2;
 float tijd = 0.005;
-float time_in_state;
-int need_to_move_1;                     // Does the robot needs to move in the first direction?
-int need_to_move_2;                     // Does the robot needs to move in the second direction?
+float time_in_state;                
+int need_to_move_1;                 // Does the robot needs to move in the first direction?
+int need_to_move_2;                 // Does the robot needs to move in the second direction?
 volatile double EMG_calibrated_max_1 = 2.00000;  // Maximum value of the first EMG signal found in the calibration state.
 volatile double EMG_calibrated_max_2 = 2.00000;  // Maximum value of the second EMG signal found in the calibration state.
-volatile double emg1_filtered;              //measured value of the first emg
-volatile double emg2_filtered;              //measured value of the second emg
-const double x0 = 80.0; //zero x position after homing
-const double y0 = 141.0; //zero y position after homing
-volatile double  setpointx = x0;
-volatile double  setpointy = y0;
-volatile int sx;//value of the button and store as switch
-volatile int sy;//value of the button and store as switch
-double dirx = 1.0; //determine the direction of the setpoint placement
-double diry = 1.0; //determine the direction of the setpoint placement
-volatile double  U1;
-volatile double  U2;
+volatile double emg1_filtered;      //measured value of the first emg
+volatile double emg2_filtered;      //measured value of the second emg
+const double x0 = 80.0;             //zero x position after homing
+const double y0 = 141.0;            //zero y position after homing
+volatile double  setpointx = x0;    // Set setpoint to zero position
+volatile double  setpointy = y0;    // Set setpoint to zero position
+volatile int sx;                    //value of the button and store as switch
+volatile int sy;                    //value of the button and store as switch
+double dirx = 1.0;                  //determine the direction of the setpoint placement
+double diry = 1.0;                  //determine the direction of the setpoint placement
+volatile double  U1;                // Motor voltage for motor 1        
+volatile double  U2;                // Motor voltage for motor 2
 
 // Inverse Kinematics
 volatile double q1_diff;
 volatile double q2_diff;
-const double sq = 2.0; //to square numbers
-const double L1 = 250.0; //length of the first link
-const double L3 = 350.0; //length of the second link
+const double sq = 2.0;              // to square numbers
+const double L1 = 250.0;            // length of the first link
+const double L3 = 350.0;            // length of the second link
 
 // Reference angles of the starting position
 double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
@@ -98,42 +101,42 @@
 double q2_0_enc = q2_0 - q1_0;
 
 // DEMO
-double  point1x = 200.0;
+double  point1x = 200.0;            // Coordinates of first prescribed point
 double  point1y = 200.0;
-double  point2x = 350.0;
+double  point2x = 350.0;            // Coordinates of second prescribed point
 double  point2y = 200.0;
-double  point3x = 350.0;
+double  point3x = 350.0;            // Coordinates of third prescribed point
 double  point3y = 0;
-double  point4x = 200.0;
+double  point4x = 200.0;            // Coordinates of fourth prescribed point
 double  point4y = 0;
-volatile int track = 1;
+volatile int track = 1;             // Start with the track from zero to first prescribed point
 
-// Motoraansturing met knopjes
-const double v=0.1; //moving speed of setpoint 
+// Motorcontrol with buttons
+const double v=0.1;                 // moving speed of setpoint 
 
-double potwaarde1;
+double potvalue1;
 double pot1;
-double potwaarde2;
+double potvalue2;
 double pot2;
 
 // Determine demo setpoints
 const double stepsize1 = 0.15;
 const double stepsize2 = 0.25;
-const double setpoint_error = 0.3;
+const double setpoint_error = 0.3;  
 
 // ----------------------------------------------
 // ------- FUNCTIONS ----------------------------
 // ----------------------------------------------
 
 // Encoders
-void ReadEncoder1()            // Read Encoder of motor 1.
+void ReadEncoder1()                              // Read Encoder of motor 1.
 {
     counts1 = Encoder1.getPulses();              // Counts of outputshaft of motor 1
     theta1 = (float(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
     vel_1 = (theta1 - theta1_prev) / tijd;       // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
     theta1_prev = theta1;                        // Define theta_prev
 }
-void ReadEncoder2()                // Read encoder of motor 2.
+void ReadEncoder2()                              // Read encoder of motor 2.
 {
     counts2 = Encoder2.getPulses();              // Counts of outputshaft of motor 2
     theta2 = (float(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
@@ -143,25 +146,25 @@
 
 double  counts2angle1()
 {
-    counts1 = Encoder1.getPulses();         // Counts of outputshaft of motor 1
-    theta1 = -(double(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
+    counts1 = Encoder1.getPulses();              // Counts of outputshaft of motor 1
+    theta1 = -(double(counts1)/4200) * 2*pi;     // Angle of outputshaft of motor 1
     return theta1;
 }
 
 double counts2angle2()
 {
-    counts2 = Encoder2.getPulses();         // Counts of outputshaft of motor 2
+    counts2 = Encoder2.getPulses();               // Counts of outputshaft of motor 2
     theta2 = (double(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
     return theta2;
 }
 
 // Motor calibration
-void MotorAngleCalibrate()                  // Function that drives motor 1 and 2.
+void MotorAngleCalibrate()                      // Function that drives motor 1 and 2.
 {
-        float U1 = -0.2;            // Negative, so arm goes backwards.
-        float U2 = -0.2;             // Motor 2 is not taken into account yet.
+        float U1 = -0.2;                        // Negative, so arm goes backwards.
+        float U2 = -0.2;                        // Motor 2 is not taken into account yet.
         
-        motor1_pwm.write(fabs(U1));         // Send PWM values to motor
+        motor1_pwm.write(fabs(U1));             // Send PWM values to motor
         motor2_pwm.write(fabs(U2));
         
         directionM1 = U1 > 0.0f;            // Either true or false, determines direction.
@@ -204,8 +207,9 @@
         }
 }
 */
-
-// Inverse Kinematics
+// ----------------------------------------------
+// ------- INVERSE KINEMATICS -------------------
+// ----------------------------------------------
 double makeAngleq1(double x, double y){
     double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
     q1_diff = -2.0*(q1-q1_0);                                                                                    //the actual amount of radians that the motor has to turn in total to reach the setpoint
@@ -219,20 +223,24 @@
     q2_diff = (2.0*(q2_motor - q2_0_enc)); //the actual amount of radians that the motor has to turn in total to reach the setpoint
     return q2_diff;
 }
+// ----------------------------------------------
+// ------- PI CONTROLLLERS ----------------------
+// ----------------------------------------------
+// Two PI controllers are defined. One used to return U1 and one to return U2.
+// These two could be combined in the future.
 
-// PI controllers
 double PID_controller1(double error1)
 {
     static double error_integral1 = 0;
     static double error_prev1 = error1; // initialization with this value only done once!
 
     // Proportional part
-    double Kp1 = 20.0;              // Kp (proportionele controller, nu nog een random waarde)
-    double u_p1 = Kp1*error1;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
+    double Kp1 = 20.0;                  // Kp (proportionele controller, nu nog een random waarde)
+    double u_p1 = Kp1*error1;           // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
     
     // Integral part
-    double Ki1 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
-    double Ts1 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
+    double Ki1 = 6.0;                   // Ki (Integrale deel vd controller, nu nog een random waarde)
+    double Ts1 = 0.005;                 // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
     error_integral1 = error_integral1 + error1 * Ts1;  
     double u_i1 = Ki1 * error_integral1;
     
@@ -275,16 +283,20 @@
     // Return
     return U2;      
 } 
+// ----------------------------------------------
+// ------- SETPOINT DETERMINATION ---------------
+// ----------------------------------------------
 
-// Determination of setpoint
 void determineEMGset(){
     setpointx = setpointx + dirx*need_to_move_1*v;
     setpointy = setpointy + diry*need_to_move_2*v;
     } 
 
-// Motoraansturing voor EMG signalen   
+// ----------------------------------------------
+// ------- MOTOR CONTROL ------------------------
+// ----------------------------------------------
 
-void motoraansturingEMG()
+void motorcontrolEMG()
 {
     sample();
     determineEMGset();
@@ -311,6 +323,9 @@
     directionM2 = U2 > 0.0f; 
 }
 
+// ----------------------------------------------
+// ------- DEMO FUNCTIONS -----------------------
+// ----------------------------------------------
 double determinedemosetx(double setpointx, double setpointy)
 {
 
@@ -318,7 +333,7 @@
         setpointx = setpointx + stepsize1;    
     }
     
-    // Van punt 1 naar punt 2. 
+    // From point 1 to 2
     if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 41)) {
         track = 12;
     }
@@ -327,17 +342,18 @@
         setpointx = setpointx + stepsize2;
     }
     
-    // Van punt 2 naar punt 3. 
+    // From point 2 to 3 
     if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && track == 12){
         setpointx = point3x; 
         track = 23;
     }
 
     if (setpointy > point3y && track == 23){
-        setpointx = point3x;          // Van punt 1 naar punt 2 op dezelfde x blijven. 
+        setpointx = point3x;          
+        // Stay on the same y value from point 2 to 3 
     } 
  
-    // Van punt 3 naar punt 4. 
+    // From point 3 to 4
     if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23)) {
         setpointy = point4y;
         track = 34;
@@ -347,35 +363,37 @@
         setpointx = setpointx - stepsize2;
     }
     
-    // Van punt 4 naar punt 1.        
+    // From point 4 to 1     
     if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34)){
         setpointx = point4x;
         track = 41;
     }
     
     if (setpointy < point1y && track == 41){
-        setpointx = point4x;        // Van punt 4 naar punt 2 op dezelfde x blijven.
+        setpointx = point4x;        
+        // From point 4 to 1, stay on the same x value
     }
     return setpointx;
 }  
 
 double determinedemosety(double setpointx, double setpointy)
 {
-    // Van reference positie naar punt 1.
+    // From reference to point 1
     if(setpointy < point1y && track == 1){
         setpointy = setpointy + (stepsize2);
     } 
 
-    // Van punt 1 naar punt 2. 
+    // From point 1 to 2
     if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 41)){
-        setpointy = point2y;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        setpointy = point2y;          
+        // Stay on the same y from point 1 to 2. 
         track = 12;
     }
     if (setpointx < point2x && track == 12){
         setpointy = point2y;
     }
     
-    // Van punt 2 naar punt 3. 
+    // From point 2 to 3
     if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && (track == 12)){
         setpointx = point3x;
         track = 23;
@@ -385,7 +403,7 @@
         track = 23;
     }    
     
-    // Van punt 3 naar punt 4. 
+    // From point 3 to 4
     if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23)){
         setpointy = setpointy;
         track = 34;
@@ -394,26 +412,27 @@
         setpointy = setpointy;
     }     
     
-    // Van punt 4 naar punt 1.  
+    // From point 4 to 1
     if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34)){
         track = 41;
     }
     
     if (setpointy < point1y && track == 41){
-        setpointy = setpointy + (stepsize2);        // Van punt 4 naar punt 2 op dezelfde x blijven.
+        setpointy = setpointy + (stepsize2);        
+        // From point 4 to 1, stay on the same x value.
     }
     return setpointy;
     
 }
 
-void determineknopjesset() {
+void determinebuttonsset() {
     setpointx = setpointx + dirx*sx*v;
     setpointy = setpointy + diry*sy*v;
     }
     
-void motoraansturingknopjes()
+void motorcontrolbuttons()
 {
-    determineknopjesset();
+    determinebuttonsset();
     q1_diff = makeAngleq1(setpointx, setpointy);
     q2_diff = makeAngleq2(setpointx, setpointy);
     //q1_diff_final = makeAngleq1(xfinal, yfinal);
@@ -437,7 +456,7 @@
     directionM2 = U2 > 0.0f;     
 }    
 
-void motoraansturingdemo()
+void motorcontroldemo()
 {    
     setpointx = determinedemosetx(setpointx, setpointy);
     setpointy = determinedemosety(setpointx, setpointy);
@@ -458,7 +477,7 @@
     directionM2 = U2 > 0.0f; 
 }
 
-void motoraansturinghoming()
+void motorcontrolhoming()
 {    
     setpointx = x0;
     setpointy = y0;
@@ -516,19 +535,17 @@
         timer.start();                              //Start timer to get time in the state "MOTOR_ANGLE_CLRBRT" 
         if (stateChanged)
             {
-                MotorAngleCalibrate();                      // Actuate motor 1 and 2.
+                MotorAngleCalibrate();              // Actuate motor 1 and 2.
                 ReadEncoder1();                     // Get velocity of motor 1    
                 ReadEncoder2();                     // Get velocity of motor 2
-                stateChanged = true;                        // Keep this loop going, until the transition conditions are satisfied.    
+                stateChanged = true;                // Keep this loop going, until the transition conditions are satisfied.    
             }
     
         // State transition logic
-        time_in_state = timer.read();                       // Determine if this state has run for long enough.
+        time_in_state = timer.read();               // Determine if this state has run for long enough.
     
         if(time_in_state > 2.0f && vel_1 < 1.1f && vel_2 < 1.1f)
             {
-                //pc.printf( "Tijd in deze staat = %f \n\r", time_in_state);           
-                //pc.printf( "Tijd tijdens actions loop (Waarde voor bepalen van snelheid)") = %f \n\r", tijd);
                 pc.printf("Motor calibration has ended. \n\r");
                 timer.stop();                              // Stop timer for this state
                 timer.reset();                             // Reset timer for this state
@@ -546,7 +563,8 @@
             stateChanged = true;
         }
         break; 
-/**                        
+/**             
+        This state is already programmed, but was not working properly on the robot.           
         case EMG_CLBRT:
         // In this state the person whom is connected to the robot needs 
         // to flex his/her muscles as hard as possible, in order to 
@@ -588,27 +606,27 @@
         // Robot moves to the home starting configuration
         pc.printf("HOMING \r\n");
         led_red = 0; led_green = 1; led_red = 0;                  // Colouring the led PURPLE
-        motor1_pwm.period_us(60);                // Period is 60 microseconde
+        motor1_pwm.period_us(60);                                 // Period is 60 microseconde
         motor2_pwm.period_us(60);
         
         // Actions
-        timer.start();                              //Start timer to get time in the state "MOTOR_ANGLE_CLRBRT" 
+        timer.start();                                            // Start timer to get time in the state "MOTOR_ANGLE_CLRBRT" 
         if(stateChanged){
-            motoraansturinghoming();
+            motorcontrolhoming();
             stateChanged = true;
          }   
         
     
         // State transition logic
-        time_in_state = timer.read();                       // Determine if this state has run for long enough.    
+        time_in_state = timer.read();                           // Determine if this state has run for long enough.    
         if(time_in_state > 5.0f && vel_1 < 1.1f && vel_2 < 1.1f)
             {
                 pc.printf("Homing has ended. We are now in reference position. \n\r");
-                timer.stop();                              // Stop timer for this state
-                timer.reset();                             // Reset timer for this state
-                motor1_pwm.write(fabs(0.0));               // Send PWM values to motor
+                timer.stop();                                   // Stop timer for this state
+                timer.reset();                                  // Reset timer for this state
+                motor1_pwm.write(fabs(0.0));                    // Send PWM values to motor
                 motor2_pwm.write(fabs(0.0));
-                Encoder1.reset();                          // Reset Encoders when arrived at zero-position
+                Encoder1.reset();                               // Reset Encoders when arrived at zero-position
                 Encoder2.reset();
                 track = 1;
         
@@ -667,7 +685,7 @@
         // Description:
         // In this state the robot follows a preprogrammed shape, e.g. 
         // a square.
-        motor1_pwm.period_us(60);                // Period is 60 microseconde
+        motor1_pwm.period_us(60);                   // Period is 60 microseconde
         motor2_pwm.period_us(60);
            
         led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led GREEN
@@ -678,7 +696,7 @@
                 
         // Actions
         if(stateChanged){
-            motoraansturingdemo();
+            motorcontroldemo();
             stateChanged = true;
          }   
         
@@ -696,21 +714,21 @@
         }
         break;
         
-        case MOVE_W_KNOPJES:
+        case MOVE_W_BUTTONS:
         
-        motor1_pwm.period_us(60);                // Period is 60 microseconde
+        motor1_pwm.period_us(60);                   // Period is 60 microseconde
         motor2_pwm.period_us(60);
         led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
         
         // Actions
         if (stateChanged) {
-            motoraansturingknopjes();
+            motorcontrolbuttons();
             stateChanged = true;
         }
         
-        potwaarde1 = potmeter1.read();  // Lees de potwaardes uit
+        potvalue1 = potmeter1.read();               // Read the first potvalue
       
-        pot1 = potwaarde1*2 - 1;   // Scale van -1 tot 1 ipv. 0 tot 1
+        pot1 = potvalue1*2 - 1;                     // Scale from -1 to 1 instead of 0 to 1
         if (pot1 <  0)  {
             dirx = -1;
         }
@@ -718,9 +736,9 @@
             dirx = 1;
         }
         
-        potwaarde2 = potmeter2.read();  // Lees de potwaardes uit
+        potvalue2 = potmeter2.read();               // Read the second potvalue
       
-        pot2 = potwaarde2*2 - 1;   // Scale van -1 tot 1 ipv. 0 tot 1
+        pot2 = potvalue2*2 - 1;                     // Scale from -1 to 1 instead of 0 to 1
         if (pot2 <  0)  {
             diry = -1;
         }
@@ -728,8 +746,8 @@
             diry = 1;
         }
         
-        sx = !buttonx.read(); //this has to be replaced with the input from the EMG, this then functions like a button
-        sy = !buttony.read(); //this has to be replaced with the input from the EMG, this then functions like a button  
+        sx = !buttonx.read(); //this has to be replaced with the input from the EMG, this then function is like a button
+        sy = !buttony.read(); //this has to be replaced with the input from the EMG, this then function is like a button  
             
         // State transition
         if (button_clbrt_home == 0) 
@@ -750,13 +768,13 @@
           
         case MOVE_W_EMG:
         
-        motor1_pwm.period_us(60);                // Period is 60 microseconde
+        motor1_pwm.period_us(60);                   // Period is 60 microseconde
         motor2_pwm.period_us(60);
         led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
         
         // Actions
         if (stateChanged) {
-            motoraansturingEMG();
+            motorcontrolEMG();
             stateChanged = true;
         }
       /*
@@ -789,7 +807,7 @@
         // This state is reached when the failure button is reached. 
         // In this state everything is turned off.
             
-        led_red = 0; led_green = 1; led_blue = 1;   // Colouring the led RED
+        led_red = 0; led_green = 1; led_blue = 1;       // Colouring the led RED
         // Actions
         if (stateChanged)
         {
@@ -845,14 +863,12 @@
     wait(0.5f);
     pc.printf("WAITING... \r\n");
     
-    //sample.attach(&ReadEMG, 0.02f);
+    //sample.attach(&ReadEMG, 0.02f); // Can be used to print the EMG values
     StateMachine.attach(&ProcessStateMachine, 0.005f);   // Run statemachine 200 times per second
     
-    
+    // Interrupt when the buttons are pushed
     InterruptIn buttondirx(D2);
-    
     InterruptIn buttondiry(D3);
-    
         
     while (true) {
         
@@ -861,17 +877,17 @@
             pc.printf("Setpointx: %0.2f, Setpointy: %0.2f, q1_diff: %0.2f, q2_diff: %0.2f, error1: %0.2f, error2: %0.2f, U1: %0.2f, U2: %0.2f\r\n", setpointx,setpointy,q1_diff,q2_diff,error1,error2,U1,U2);
             
             if (track == 1) {
-                pc.printf("Gaat naar positie 1\r\n");
+                pc.printf("Go to position 1\r\n");
             }
             else if (track == 12) {
-                pc.printf("Gaat naar positie 2\r\n");
+                pc.printf("Go to position 2\r\n");
             }
         
             else if (track == 23) {
-                pc.printf("Gaat naar positie 3\r\n");
+                pc.printf("Go to position 3\r\n");
             }
             else if (track == 34) {
-                pc.printf("Gaat naar positie 4\r\n");
+                pc.printf("Go to position 4\r\n");
             }
         }