Diff: main.cpp

Revision:

12:3e084e1a699e

Parent:

10:3f93fdb90c29

Child:

13:a2e281d5de89

diff -r 6f1fa9cc2346 -r 3e084e1a699e main.cpp
--- a/main.cpp	Wed Oct 31 14:34:57 2018 +0000
+++ b/main.cpp	Thu Nov 01 17:53:13 2018 +0000
@@ -3,9 +3,6 @@
 #include "QEI.h"
 #include "FastPWM.h"
 #include "math.h"
-
-#include "mbed.h"
-//#include "HIDScope.h"
 #include "BiQuad.h"
 #include "BiQuad4.h"
 #include "FilterDesign.h"
@@ -17,8 +14,8 @@
 DigitalOut led_blue(LED_BLUE);
 // Buttons
 DigitalIn button_clbrt_home(SW2);
-DigitalIn button_Demo(D5);
-DigitalIn button_Emg(D6);
+DigitalIn button_Demo(D2);
+DigitalIn button_Emg(D3);
 DigitalIn Fail_button(SW3);
 // Modserial
 MODSERIAL pc(USBTX, USBRX);
@@ -33,8 +30,6 @@
 // EMG input en start value of filtered EMG
 AnalogIn    emg1_raw( A0 );
 AnalogIn    emg2_raw( A1 );
-double emg1_filtered = 0.00;
-double emg2_filtered = 0.00;
 float threshold_EMG = 0.25;         // Threshold on 25 percent of the maximum EMG
 
 // Declare timers and Tickers
@@ -43,15 +38,16 @@
 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
 
 // Set up ProcessStateMachine
 enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_DEMO, FAILURE_MODE};
 states currentState = WAITING;
 bool stateChanged = true;
-volatile bool writeVelocityFlag = false;
 
 // Global variables
 char c;
+const float fs = 1/1024;
 int counts1; 
 int counts2;
 float theta1;
@@ -63,34 +59,72 @@
 const float pi = 3.14159265359;
 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?
-double EMG_calibrated_max_1 = 2.00000;  // Maximum value of the first EMG signal found in the calibration state.
-double EMG_calibrated_max_2 = 2.00000;  // Maximum value of the second EMG signal found in the calibration state.
-double emg1_cal = 0.00000;              //measured value of the first emg
-double emg2_cal = 0.00000;              //measured value of the second emg
+volatile double EMG_calibrated_max_1 = 0.00000;  // Maximum value of the first EMG signal found in the calibration state.
+volatile double EMG_calibrated_max_2 = 0.00000;  // Maximum value of the second EMG signal found in the calibration state.
+volatile double emg1_cal;              //measured value of the first emg
+volatile double emg2_cal;              //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;
+
+// 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
+
+// Reference angles of the starting position
+double q2_0 = -acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
+double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
+double q2_0_enc = (pi-q2_0) - q1_0;
+
+// DEMO
+double  point1x = 200.0;
+double  point1y = 200.0;
+double  point2x = 350.0;
+double  point2y = 200.0;
+double  point3x = 350.0;
+double  point3y = 0;
+double  point4x = 200.0;
+double  point4y = 0;
+volatile int track = 1;
+
+// Determine demo setpoints
+const double stepsize1 = 0.15;
+const double stepsize2 = 0.25;
+const double setpoint_error = 0.3;
 
 // ----------------------------------------------
 // ------- FUNCTIONS ----------------------------
 // ----------------------------------------------
 
-float ReadEncoder1()            // Read Encoder of motor 1.
+// Encoders
+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
-    return vel_1;
 }
-float 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
     vel_2 = (theta2 - theta2_prev) / tijd;       // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
     theta2_prev = theta2;                        // Define theta_prev
-    return vel_2;
 }
+
+// Motor calibration
 void MotorAngleCalibrate()                  // Function that drives motor 1 and 2.
 {
         float U1 = -0.2;            // Negative, so arm goes backwards.
@@ -102,19 +136,251 @@
         directionM1 = U1 > 0.0f;            // Either true or false, determines direction.
         directionM2 = U2 > 0.0f;
 } 
-void sample()
+
+// Read EMG
+void ReadEMG()
+{
+    emg1_cal = FilterDesign(emg1_raw.read());
+    emg2_cal = FilterDesign2(emg2_raw.read());
+    pc.printf("emg1_cal = %g, emg2_cal = %g \n\r", emg1_cal, emg2_cal);
+}
+
+// EMG calibration
+void EMG_calibration()
+{
+
+    for (int i = 0; i <= 10; i++) //10 measuring points
+        {
+        ReadEMG();
+        if (emg1_cal > EMG_calibrated_max_1){
+            EMG_calibrated_max_1 = emg1_cal;}
+            
+        if (emg2_cal > EMG_calibrated_max_2){
+            EMG_calibrated_max_2 = emg2_cal;}
+            
+        pc.printf("EMG1_max = %f, EMG2_max = %f \r\nEMG1_filtered = %f \r\nEMG2_filtered = %f \r\n",EMG_calibrated_max_1,EMG_calibrated_max_2, emg1_cal, emg2_cal);
+        wait(0.5f);
+        }
+}
+
+// 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
+    return q1_diff;
+}
+
+double makeAngleq2(double x, double y){
+    double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration
+    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
+    double q2_motor = (pi - q2) - q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
+    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 controllers
+double PI_controller1(double error1)
 {
-    emg1_filtered = FilterDesign(emg1_raw.read());
-    emg2_filtered = FilterDesign2(emg2_raw.read());
+    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)
+    
+    // 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)   
+    error_integral1 = error_integral1 + error1 * Ts1;  
+    double u_i1 = Ki1 * error_integral1;
+    
+    // Derivative part
+    double Kd1 = 2.0;
+    double error_derivative1 = (error1 - error_prev1)/Ts1; 
+    double u_d1 = Kd1 * error_derivative1; 
+    error_prev1 = error1;
+    
+    // Sum 
+    U1 = u_p1 + u_i1 + u_d1;
+    
+    // Return
+    return U1;      
+}
+double PI_controller2(double error2)
+{
+    static double error_integral2 = 0;
+    static double error_prev2 = error2; // initialization with this value only done once!
+
+    // Proportional part
+    double Kp2 = 20.0;              // Kp (proportionele controller, nu nog een random waarde)
+    double u_p2 = Kp2*error2;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
+    
+    // Integral part
+    double Ki2 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
+    double Ts2 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
+    error_integral2 = error_integral2 + error2 * Ts2;  
+    double u_i2 = Ki2 * error_integral2;
+    
+    // Derivative part
+    double Kd2 = 2.0; 
+    double error_derivative2 = (error2 - error_prev2)/Ts2; 
+    double u_d2 = Kd2 * error_derivative2; 
+    error_prev2 = error2;
     
-    /**
-    scope.set(0, emg1_raw.read());      // Raw EMG 1 send to scope 0
-    scope.set(1, emg1_filtered);        // Filtered EMG 1 send to scope 1
-    scope.set(2, emg2_raw.read());      // Raw EMG 2 send to scope 2
-    scope.set(3, emg2_filtered);        // Filtered EMG 2 send to scope 3
-    scope.send();                       // Send the data to the computer
-    */
+    // Sum +
+    U2 = u_p2 + u_i2 + u_d2;
+
+    // Return
+    return U2;      
+} 
+
+// Determination of setpoint
+void determineEMGset(){
+    const double v = 0.1; //moving speed of setpoint 
+    setpointx = setpointx + dirx*sx*v;
+    setpointy = setpointy + diry*sy*v;
+    }
+void ChangeDirectionX(){
+    dirx = -1*dirx;
+    }
+void ChangeDirectionY(){
+    diry = -1*diry;
+    }  
+
+// Motoraansturing voor EMG signalen   
+/** 
+void motoraansturing()
+{
+    determineEMGset();
+    q1_diff = makeAngleq1(setpointx, setpointy);
+    q2_diff = makeAngleq2(setpointx, setpointy);
+    ReadEncoder1();
+    ReadEncoder2();           
+    double error2 = q2_diff - theta2; 
+    double error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.
+    U1 = PI_controller1(error1);               // Voltage dat naar de motor gestuurd wordt. 
+    U2 = PI_controller2(error2);   
+    pc.printf("U1 = %g, U2 = %g \n\r", U1, U2);
+    motor1_pwm.write(fabs(U1));                // Motor aansturen
+    directionM1 = U1 > 0.0f;                   // Richting van de motor bepalen
+    motor2_pwm.write(fabs(U2));
+    directionM2 = U2 > 0.0f; 
 }
+**/ 
+double determinedemosetx(double setpointx, double setpointy)
+{
+
+    if (setpointx < point1x && track == 1){ 
+        setpointx = setpointx + stepsize1;    
+    }
+    
+    // Van punt 1 naar punt 2. 
+    if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 41)) {
+        track = 12;
+    }
+    
+    if (setpointx < point2x && track == 12){
+        setpointx = setpointx + stepsize2;
+    }
+    
+    // Van punt 2 naar punt 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. 
+    } 
+ 
+    // Van punt 3 naar punt 4. 
+    if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23)) {
+        setpointy = point4y;
+        track = 34;
+    }
+    
+    if (setpointx > point4x && track == 34){
+        setpointx = setpointx - stepsize2;
+    }
+    
+    // Van punt 4 naar punt 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.
+    }
+    return setpointx;
+}  
+
+double determinedemosety(double setpointx, double setpointy)
+{
+    // Van reference positie naar punt 1.
+    if(setpointy < point1y && track == 1){
+        setpointy = setpointy + (stepsize2);
+    } 
+
+    // Van punt 1 naar punt 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. 
+        track = 12;
+    }
+    if (setpointx < point2x && track == 12){
+        setpointy = point2y;
+    }
+    
+    // Van punt 2 naar punt 3. 
+    if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && (track == 12)){
+        setpointx = point3x;
+        track = 23;
+    }
+    if ((setpointy > point3y) && (track == 23)){
+        setpointy = setpointy + (-stepsize2);
+        track = 23;
+    }    
+    
+    // Van punt 3 naar punt 4. 
+    if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23)){
+        setpointy = setpointy;
+        track = 34;
+    }
+    if (setpointx > point4x && track == 34){
+        setpointy = setpointy;
+    }     
+    
+    // Van punt 4 naar punt 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.
+    }
+    return setpointy;
+    
+}
+void motoraansturingdemo()
+{
+    setpointx = determinedemosetx(setpointx, setpointy);
+    setpointy = determinedemosety(setpointx, setpointy);
+    q1_diff = makeAngleq1(setpointx, setpointy);
+    q2_diff = makeAngleq2(setpointx, setpointy);
+
+    ReadEncoder1();
+    ReadEncoder2();          
+    double error2 = q2_diff - theta2; 
+    double error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.
+
+    U1 = PI_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
+    U2 = PI_controller2(error2);   
+    
+    motor1_pwm.write(fabs(U1));         // Motor aansturen
+    directionM1 = U1 > 0.0f;            // Richting van de motor bepalen
+    motor2_pwm.write(fabs(U2));
+    directionM2 = U2 > 0.0f; 
+} 
 // ---------------------------------------------------
 // --------STATEMACHINE------------------------------- 
 // --------------------------------------------------- 
@@ -154,8 +420,8 @@
         if (stateChanged)
             {
                 MotorAngleCalibrate();                      // Actuate motor 1 and 2.
-                vel_1 = ReadEncoder1();                     // Get velocity of motor 1    
-                vel_2 = ReadEncoder2();                     // Get velocity of motor 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.    
             }
     
@@ -174,8 +440,7 @@
                 Encoder1.reset();                          // Reset Encoders when arrived at zero-position
                 Encoder2.reset();
         
-                currentState = EMG_CLBRT;                   // Switch to next state (EMG_CLRBRT).
-                pc.printf("EMG calibration \r\n");
+                currentState = HOMING;                   // Switch to next state (EMG_CLRBRT).
                 stateChanged = true;                       
            }
         if (Fail_button == 0)
@@ -184,60 +449,81 @@
             stateChanged = true;
         }
         break; 
-                        
+/**                        
         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 
         // measure the maximum EMG-signal, which can be used to scale 
         // the EMG-filter.
-                
-         led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
-                
-         // Requirements to move to the next state:
-         // If enough time has passed (5 sec), and the EMG-signal drops below 10%
-         // of the maximum measured value, we move to the Homing state.
-                    
-        for (int i = 0; i <= 10; i++) //10 measuring points
-        {      
-        if (emg1_cal > EMG_calibrated_max_1){
-            EMG_calibrated_max_1 = emg1_cal;}
-            
-        if (emg2_cal > EMG_calibrated_max_2){
-            EMG_calibrated_max_2 = emg2_cal;}
-            
-        //pc.printf("EMG1 = %f, EMG2 = %f \r\n",EMG_calibrated_max_1,EMG_calibrated_max_2);
-        wait(0.5f);
+
+  
+        led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
+
+        // Actions
+        if (stateChanged)
+        {
+            pc.printf("Starting EMG calibration. Contract muscles until the calibration is ended.\n\r");
+           // motor1_pwm.write(fabs(0.0));               // Send PWM values to motor
+           // motor2_pwm.write(fabs(0.0));
+            EMG_calibration();
+            pc.printf("Final: EMG1 = %f, EMG2 = %f \r\n",EMG_calibrated_max_1,EMG_calibrated_max_2);
+            stateChanged = false;
         }
         
-         currentState = HOMING;
+        // State change logic
+        
+        if (currentState == EMG_CLBRT && stateChanged == false){
+            pc.printf("EMG calibration has ended. \n\r");
+            currentState = WAITING4SIGNAL;
+            stateChanged = true;
+        }
         if (Fail_button == 0)
         {
             currentState = FAILURE_MODE;
             stateChanged = true;
-        }
-         break;       
-                            
+        }    
+        
+        break;     
+**/          
+                           
         case HOMING:
         // Description:
         // 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
-                
-        // Requirements to move to the next state:
-        // If we are in the right location, within some margin, we move to the Waiting for
-        // signal state.
-                
-        wait(5.0f); // time_in_this_state > 5.0f
-        // INSERT MOVEMENT
-        currentState = WAITING4SIGNAL;
+
+        // Actions
+        timer.start();                              //Start timer to get time in the state "MOTOR_ANGLE_CLRBRT" 
+        if (stateChanged)
+            {
+                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.    
+            }
+    
+        // State transition logic
+        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("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
+                motor2_pwm.write(fabs(0.0));
+                Encoder1.reset();                          // Reset Encoders when arrived at zero-position
+                Encoder2.reset();
+        
+                currentState = WAITING4SIGNAL;                   // Switch to next state (EMG_CLRBRT).
+                stateChanged = true;                       
+           }        
         if (Fail_button == 0)
-        {
-            currentState = FAILURE_MODE;
-            stateChanged = true;
-        }
+            {
+                currentState = FAILURE_MODE;
+                stateChanged = true;
+            }
         break;
-                
+               
         case WAITING4SIGNAL:
         // Description:
         // In this state the robot waits for an action to occur.
@@ -257,13 +543,15 @@
         else if (button_Demo == 1) 
         {
             currentState = MOVE_W_DEMO;
-            pc.printf("DEMO \r\n");
+            stateChanged = true;
+            pc.printf("DEMO mode \r\n");
             wait(1.0f);
         }
         else if (button_Emg == 1) 
         {
             currentState = MOVE_W_EMG;
-            pc.printf("EMG \r\n");
+            stateChanged = true;
+            pc.printf("EMG mode\r\n");
             wait(1.0f);
         }
         else if (Fail_button == 0)
@@ -273,7 +561,8 @@
         }
         
         break;
-                
+        
+      
         case MOVE_W_DEMO:
         // Description:
         // In this state the robot follows a preprogrammed shape, e.g. 
@@ -285,8 +574,13 @@
         // When the home button or the failure button is pressed, we 
         // will the move to the corresponding state.
                 
-        // BUILD DEMO MODE
+        // Actions
+        if(stateChanged){
+            motoraansturingdemo();
+            stateChanged = true;
+         }   
         
+        // State transition
         if (button_clbrt_home == 0) 
         {
             currentState = HOMING;
@@ -299,31 +593,38 @@
             stateChanged = true;
         }
         break;
-                
+ 
+/**           
         case MOVE_W_EMG:
         // Description:
         // In this state the robot will be controlled by use of 
         // EMG-signals. 
-                    
+        
+        // Actions                    
         led_red = 1; led_green = 0; led_blue = 1;   // Colouring the led GREEN
-             
-        if (emg1_filtered >= (threshold_EMG*EMG_calibrated_max_1)){
-            need_to_move_1 = 1; // The robot does have to move
+        ReadEMG();
+        if (stateChanged){
+        //ReadEMG();
+        //pc.printf(" emg1 = %g, emg2 = %g \n\r ", emg1_cal, emg2_cal);     
+        if (emg1_cal >= (threshold_EMG*EMG_calibrated_max_1)){
+            sx = 1; // The robot does have to move
             }
         else {
-            need_to_move_1 = 0; // If the robot does not have to move
+            sx = 0; // If the robot does not have to move
             }
 
-        if(emg2_filtered >= threshold_EMG*EMG_calibrated_max_2){
-            need_to_move_2 = 1;
+        if(emg1_cal >= threshold_EMG*EMG_calibrated_max_2){
+            sy = 1;
             }
         else {
-            need_to_move_2 = 0;
+            sy = 0;
             }   
-        // Requirements to move to the next state:
-        // When the home button or the failure button is pressed, we 
-        // will the move to the corresponding state.
+            
+        motoraansturing();     
+        stateChanged = true;
+        }
         
+        // State transition logic
         if (button_clbrt_home == 0) 
         {
             currentState = MOTOR_ANGLE_CLBRT;
@@ -335,8 +636,9 @@
             currentState = FAILURE_MODE;
             stateChanged = true;
         }
-        break;      
-                 
+        break; 
+**/             
+                
         case FAILURE_MODE:
         // Description:
         // This state is reached when the failure button is reached. 
@@ -376,10 +678,11 @@
             wait(0.2f);
             }
             wait(0.4f);
-        }
+        }              
     }
+    
 }
-            
+           
  // --------------------------------  
  // ----- MAIN LOOP ----------------
  // --------------------------------  
@@ -393,16 +696,20 @@
     
     pc.baud(115200);
     
-    pc.printf("\r\n _______________ INSERT ROBOT NAME HERE! _______________ \r\n");
+    pc.printf("\r\n _______________ FEED ME! _______________ \r\n");
     wait(0.5f);
     pc.printf("WAITING... \r\n");
     
+    //sample.attach(&ReadEMG, 0.02f);
     StateMachine.attach(&ProcessStateMachine, 0.005f);   // Run statemachine 200 times per second
-    /**
-    sample_EMGtoHIDscope.attach(&sample, 0.02f);         // Display EMG values 50 times per second
-    */
+    
+    
+    InterruptIn directionx(SW3);
+    directionx.fall(ChangeDirectionX); //change the direction of the setpoint in x direction
+    InterruptIn directiony(SW2);
+    directiony.fall(ChangeDirectionY); //change the direction of the setpoint in y direction
+        
     while (true) {
-        
     }
 }