File content as of revision 14:059fd8f6cbfd:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "FastPWM.h"
#include "math.h"
#include "BiQuad.h"
#include "BiQuad4.h"
#include "FilterDesign.h"
#include "FilterDesign2.h"

// LED's
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 Fail_button(SW3);
// 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 directionM2(D7);
FastPWM motor1_pwm(D5);
FastPWM motor2_pwm(D6);
// EMG input en start value of filtered EMG
AnalogIn    emg1_raw( A0 );
AnalogIn    emg2_raw( A1 );
AnalogIn  potmeter1(PTC11);
AnalogIn potmeter2(PTC10);

// Declare timers and Tickers
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

// 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;

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;
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?
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 = pi + 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 = 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;

// Motoraansturing met knopjes
const double v=0.1; //moving speed of setpoint 

double potwaarde1;
double pot1;
double potwaarde2;
double pot2;

// Determine demo setpoints
const double stepsize1 = 0.15;
const double stepsize2 = 0.25;
const double setpoint_error = 0.3;

// ----------------------------------------------
// ------- FUNCTIONS ----------------------------
// ----------------------------------------------

// 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
}
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
}

double  counts2angle1()
{
    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
    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.
{
        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
        motor2_pwm.write(fabs(U2));
        
        directionM1 = U1 > 0.0f;            // Either true or false, determines direction.
        directionM2 = U2 > 0.0f;
} 

// 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 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)
    
    // 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 PID_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;
    
    // 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 determineknopjesset() {
    setpointx = setpointx + dirx*sx*v;
    setpointy = setpointy + diry*sy*v;
    }
    
void motoraansturingknopjes()
{
    determineknopjesset();
    q1_diff = makeAngleq1(setpointx, setpointy);
    q2_diff = makeAngleq2(setpointx, setpointy);
    //q1_diff_final = makeAngleq1(xfinal, yfinal);
    //q2_diff_final = makeAngleq2(xfinal, yfinal);

    theta2 = counts2angle2();           
    error2 = q2_diff - theta2;
    theta1 = counts2angle1();  
    error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.
    
    //errors die de setpoints bepalen
    //error1_final = q1_diff_final - theta1;
    //error2_final = q2_diff_final - theta2;

    U1 = PID_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
    U2 = PID_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;     
}    

void motoraansturingdemo()
{    
    setpointx = determinedemosetx(setpointx, setpointy);
    setpointy = determinedemosety(setpointx, setpointy);
    q1_diff = makeAngleq1(setpointx, setpointy);
    q2_diff = makeAngleq2(setpointx, setpointy);

    theta2 = counts2angle2();           
    error2 = q2_diff - theta2;
    theta1 = counts2angle1();  
    error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.

    U1 = PID_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
    U2 = PID_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; 
}

void motoraansturinghoming()
{    
    setpointx = x0;
    setpointy = y0;
    q1_diff = makeAngleq1(setpointx, setpointy);
    q2_diff = makeAngleq2(setpointx, setpointy);

    theta2 = counts2angle2();           
    error2 = q2_diff - theta2;
    theta1 = counts2angle1();  
    error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.

    U1 = PID_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
    U2 = PID_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------------------------------- 
// --------------------------------------------------- 
void ProcessStateMachine(void)
{
    switch (currentState)
    {
        case WAITING:
        // Description:
        // In this state we do nothing, and wait for a command
        
        // Actions        
        led_red = 0; led_green = 0; led_blue = 0;   // Colouring the led WHITE
        
        // State transition logic
        if (button_clbrt_home == 0) 
        {
            currentState = MOTOR_ANGLE_CLBRT;
            stateChanged = true;
            pc.printf("Starting Calibration\n\r");
        }
        else if (Fail_button == 0)
        {
            currentState = FAILURE_MODE;
            stateChanged = true;
        }
        break;
                
        case MOTOR_ANGLE_CLBRT:
        // Description:
        // In this state the robot moves with low motor PWM to some 
        // mechanical limit of motion, in order to calibrate the motors.
         
        // Actions       
        led_red = 1; led_green = 0; led_blue = 0;   // Colouring the led TURQUOISE
        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( "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
                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 = HOMING;                   // Switch to next state (EMG_CLRBRT).
                stateChanged = true;                       
           }
        if (Fail_button == 0)
        {
            currentState = FAILURE_MODE;
            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

        // 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;
        }
        
        // 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;     
**/          
                           
        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
        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" 
        if(stateChanged){
            motoraansturinghoming();
            stateChanged = true;
         }   
        
    
        // State transition logic
        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
                motor2_pwm.write(fabs(0.0));
                Encoder1.reset();                          // Reset Encoders when arrived at zero-position
                Encoder2.reset();
                track = 1;
        
                currentState = WAITING4SIGNAL;                   // Switch to next state (EMG_CLRBRT).
                stateChanged = true;                       
           }        
        if (Fail_button == 0)
            {
                currentState = FAILURE_MODE;
                stateChanged = true;
            }
             
            
        break;
               
        case WAITING4SIGNAL:
        // Description:
        // In this state the robot waits for an action to occur.
        
        led_red = 0; led_green = 0; led_blue = 0;                // Colouring the led WHITE
                
        // Requirements to move to the next state:
        // If a certain button is pressed we move to the corresponding 
        // state (MOVE_W_DEMO, MOVE_W_EMG or SHUTDOWN)
                        
        if (button_clbrt_home == 0) 
        {
            currentState = MOTOR_ANGLE_CLBRT;
            stateChanged = true;
            pc.printf("Starting Calibration \n\r");
        }
        else if (button_Demo == 0) 
        {
            currentState = MOVE_W_DEMO;
            stateChanged = true;
            pc.printf("DEMO mode \r\n");
            wait(1.0f);
        }
        else if (button_Emg == 0) 
        {
            currentState = MOVE_W_EMG;
            stateChanged = true;
            pc.printf("EMG mode\r\n");
            wait(1.0f);
        }
        else if (Fail_button == 0)
        {
            currentState = FAILURE_MODE;
            stateChanged = true;
        }
        
        break;
        
      
        case MOVE_W_DEMO:
        // Description:
        // In this state the robot follows a preprogrammed shape, e.g. 
        // a square.
        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
                
        // 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.
                
        // Actions
        if(stateChanged){
            motoraansturingdemo();
            stateChanged = true;
         }   
        
        // State transition
        if (button_clbrt_home == 0) 
        {
            currentState = HOMING;
            stateChanged = true;
            pc.printf("Moving home\n\r");
        }
        else if (Fail_button == 0)
        {
            currentState = FAILURE_MODE;
            stateChanged = true;
        }
        break;
        
        case MOVE_W_KNOPJES:
        
        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();
            stateChanged = true;
        }
        
        potwaarde1 = potmeter1.read();  // Lees de potwaardes uit
      
        pot1 = potwaarde1*2 - 1;   // Scale van -1 tot 1 ipv. 0 tot 1
        if (pot1 <  0)  {
            dirx = -1;
        }
        else if (pot1 >= 0) {
            dirx = 1;
        }
        
        potwaarde2 = potmeter2.read();  // Lees de potwaardes uit
      
        pot2 = potwaarde2*2 - 1;   // Scale van -1 tot 1 ipv. 0 tot 1
        if (pot2 <  0)  {
            diry = -1;
        }
        else if (pot2 >= 0) {
            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  
            
        // State transition
        if (button_clbrt_home == 0) 
        {
            currentState = HOMING;
            stateChanged = true;
            pc.printf("Moving home\n\r");
        }
        else if (Fail_button == 0)
        {
            currentState = FAILURE_MODE;
            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
        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 {
            sx = 0; // If the robot does not have to move
            }

        if(emg1_cal >= threshold_EMG*EMG_calibrated_max_2){
            sy = 1;
            }
        else {
            sy = 0;
            }   
            
        motoraansturing();     
        stateChanged = true;
        }
        
        // State transition logic
        if (button_clbrt_home == 0) 
        {
            currentState = MOTOR_ANGLE_CLBRT;
            stateChanged = true;
            pc.printf("Starting Calibration \n\r");
        }        
        else if (Fail_button == 0)
        {
            currentState = FAILURE_MODE;
            stateChanged = true;
        }
        break; 
**/             
                
        case FAILURE_MODE:
        // Description:
        // 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
        // Actions
        if (stateChanged)
        {
            motor1_pwm.write(fabs(0.0));               // Stop all motors!
            motor2_pwm.write(fabs(0.0));
            pc.printf("FAILURE MODE \r\n PLEASE RESTART THE WHOLE ROBOT \r\n (and make sure this does not happen again) \r\n");
            stateChanged = false;
        }    
        break;
        
        // State transition logic
        // No state transition, you need to restart the robot.
                        
        default: 
        // This state is a default state, this state is reached when 
        // the program somehow defies all of the other states. 
                
        pc.printf("Unknown or unimplemented state reached!!! \n\r");
        led_red = 1; led_green = 1; led_blue = 1;   // Colouring the led BLACK
        for (int n = 0; n < 50; n++)                // Making an SOS signal with the RED led
        {              
            for (int i = 0; i < 6; i++) 
            { 
                led_red = !led_red;
                wait(0.6f);
            }
            wait(0.4f);
            for (int i = 0 ; i < 6; i++) 
            {
            led_red = !led_red;
            wait(0.2f);
            }
            wait(0.4f);
        }              
    }
    
}
           
 // --------------------------------  
 // ----- MAIN LOOP ----------------
 // --------------------------------  
    
int main()
{
    // Switch all LEDs off
    led_red = 1;
    led_green = 1;
    led_blue = 1;
    
    pc.baud(115200);
    
    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
    
    
    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) {
        
        if (currentState == MOVE_W_DEMO) {
            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");
            }
            else if (track == 12) {
                pc.printf("Gaat naar positie 2\r\n");
            }
        
            else if (track == 23) {
                pc.printf("Gaat naar positie 3\r\n");
            }
            else if (track == 34) {
                pc.printf("Gaat naar positie 4\r\n");
            }
        }

        wait(0.5f);
    }
}