File content as of revision 25:408c2242bdfa:

#include "mbed.h" // Use revision 119!!
#include "QEI.h" // For reading the encoder of the motors 
#include <ctime> // for the timer during the process (if needed)
#include "Servo.h" // For controlling the servo
#include "BiQuad.h"

#define SERIAL_BAUD 115200

// In- en outputs 
// -----------------------------------------------------------------------------

// EMG
AnalogIn emg0_in( A0 ); // x_direction
AnalogIn emg1_in( A1 ); // y_direction
AnalogIn emg2_in( A2 ); // changing directions

// Motor related
DigitalOut dirpin_1(D4); // direction of motor 1 (translation)
PwmOut pwmpin_1(D5); // PWM pin of motor 1
DigitalOut dirpin_2(D7); // direction of motor 2 (rotation)
PwmOut pwmpin_2(D6); // PWM pin of motor 2

// Extra stuff
DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
DigitalIn button_emergency(D8); // button for emergency mode, on bioshield
DigitalIn button_wait(SW3); // button for wait mode, on mbed 
DigitalIn button_demo(D9); // button for demo mode, on bioshield  

DigitalIn led_red(LED_RED); // red led 
DigitalIn led_green(LED_GREEN); // green led
DigitalIn led_blue(LED_BLUE); // blue led

Servo myservo(D3); // Define the servo to control (in penholder), up to start with

// Other stuff
// -----------------------------------------------------------------------------
// Define stuff like tickers etc

Ticker process_tick;
Ticker emergency;
QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); // Define the type of encoding: X4 encoding(default is X2)
QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING);
Serial pc(USBTX,USBRX);
Timer t; // For timing the time in each state (https://os.mbed.com/handbook/Timer)

// Variables 
// ----------------------------------------------------------------------------- 
// Define here all variables needed throughout the whole code 
volatile double time_overall;
volatile double time_in_state;
volatile int counts1_prev = 0;
volatile int counts2_prev = 0;
volatile int counts1;
volatile int counts2;

// EMG related
// Constants EMG filter
const double m1 = 0.5000;
const double m2 = -0.8090;
const double n0 = 0.5000;
const double n1 = -0.8090;
const double n2 = 0.0;
const double a1 = 0.9565;
const double a2 = -1.9131;
const double b0 = 0.9565;
const double b1 = -1.9112;
const double b2 = 0.9150;
const double c1 = 0.0675;
const double c2 = 0.1349;
const double d0 = 0.0675;
const double d1 = -1.1430;
const double d2 = 0.4128;
  
// Variables EMG
double emg0;
double emg1;
double emg2;
double notch0;
double notch1;
double notch2;
double high0;
double high1;
double high2;
double absolute0;
double absolute1;
double absolute2;
double low0;
double low1;
double low2;

// BiQuad values
BiQuadChain notch;
BiQuad N1( m1, m2, n0, n1, n2);
BiQuad N2( m1, m2, n0, n1, n2);
BiQuad N3( m1, m2, n0, n1, n2);
BiQuadChain highpass;
BiQuad H1( a1, a2, b0, b1, b2);
BiQuad H2( a1, a2, b0, b1, b2);
BiQuad H3( a1, a2, b0, b1, b2);
BiQuadChain lowpass;
BiQuad L1( c1, c2, d0, d1, d2);
BiQuad L2( c1, c2, d0, d1, d2);
BiQuad L3( c1, c2, d0, d1, d2);

const float T = 0.001f;

// EMG
const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated
double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array
double StoreArray0[sizeMovAg] = {}, StoreArray1[sizeMovAg] = {}, StoreArray2[sizeMovAg] = {};

//Empty arrays to calculate MovAgs
double Average0, Average1, Average2; //Outcome of MovAg
const int sizeCali = 500; //Size of array over which the Threshold will be calculated
double StoreCali0[sizeCali] = {}, StoreCali1[sizeCali] = {}, StoreCali2[sizeCali] = {};

//Empty arrays to calculate means in calibration
double Mean0, Mean1, Mean2; //Mean of maximum contraction, calculated in the calibration
double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; //Thresholds for muscles 0 to 2
int g = 0; //Part of the switch void, where the current state can be changed
int emg_calib=0; //After calibration this value will be 1, enabling the

// MOTOR_CAL
volatile double tower_1_position = 0.1; // the tower which he reaches first
volatile double tower_end_position = 0.1; // the tower which he reaches second
volatile double rotation_start_position = 0.1; // the position where the rotation will remain
volatile double position;
volatile float speed = 1.0;
volatile int dir = 0;

// RKI related
const double Ts = 0.001;// sample frequency 

// Constants motor
const double delta_t = 0.01;
const double el_1 = 370.0 / 2.0;
const double el_2 = 65.0 / 2.0;
const double pi = 3.14159265359;
const double alpha = (2.0 * pi) /(25.0*8400.0);
const double beta = (((2.0 * el_1) - (2.0 * el_2)) * 20.0 * pi) / (305.0 * 8400.0);
const double q1start = rotation_start_position * alpha;
const double q2start = tower_1_position * beta; 
const double q2end = tower_end_position * beta;

// Variables motors
volatile double desired_x;
volatile double desired_y;
volatile double out1;
volatile double out2;
volatile double vdesx;
volatile double vdesy;
volatile double q1;
volatile double q2;
volatile double MPe;
volatile double xe;
volatile double ye;
volatile double gamma;
volatile double dq1;
volatile double dq2;
volatile double dC1;
volatile double dC2;
volatile double pwm1;
volatile double pwm2;

// PID rotation constants 
volatile double Rot_Kp = 1.5;
volatile double Rot_Ki = 0.1;
volatile double Rot_Kd = 0.48;
volatile double Rot_error = 0.0;
volatile double Rot_prev_error = 0.0;

// PID translation constants
const double Trans_Kp = 0.5;
const double Trans_Ki = 0.5;
const double Trans_Kd = 0.1;
volatile double Trans_error = 0.0;
volatile double Trans_prev_error = 0.0;

// States
enum states {WAIT, MOTOR_CAL, EMG_CAL, START, OPERATING}; // states the robot can be in
states CurrentState = WAIT; // the CurrentState to start with is the WAIT state
bool StateChanged = true; // the state must be changed to go into the next state

enum direction {Pos_RB, Pos_LB, Pos_RO, Pos_LO};
direction currentdirection = Pos_RB;
bool directionchanged = true;

// Functions
// -----------------------------------------------------------------------------
 
// Encoder 
// Getting encoder information from motors
int Counts1(volatile int& a) // a = counts1
{
    counts1_prev = a;
    a =  Encoder1.getPulses();
    return a;
    }

int Counts2(volatile int& a) // a = counts2
{
    counts2_prev = a;
    a = Encoder2.getPulses(); 
    return a;
    }
    
// Servo control
// To lift the pen up, with a push of button
void servocontrol()
{
    if(button_motorcal == false) // If button is pushed, pen should go up
    {
        myservo = 0.1;
        }
    myservo = 0.0; 
    }
    
// EMG filter
// To process the EMG signal before information can be caught from it 

// Filter of the first EMG signal
void filtering()
{
    // Reading the EMG signal
    emg0 = emg0_in.read(); 
    emg1 = emg1_in.read();
    emg2 = emg2_in.read(); 
    
    // Applying a notch filter over the EMG data
    notch0 = N1.step(emg0); 
    notch1 = N2.step(emg1);
    notch2 = N3.step(emg2); 
    
    // Applying a high pass filter
    high0 = H1.step(notch0); 
    high1 = H2.step(notch1); 
    high2 = H3.step(notch2);
    
    // Rectifying the signal
    absolute0 = fabs(high0); 
    absolute1 = fabs(high1);
    absolute2 = fabs(high2); 
    
    // Applying low pass filter
    low0 = L1.step(absolute0); 
    low1 = L2.step(absolute1);
    low2 = L3.step(absolute2); 
    }

// Moving average filter
// To determine the moving average, apply after filtering
void MovAg()
{    
    // For statement to make an array of the last datapoints of the filtered signal
    for (int i = sizeMovAg - 1; i >= 0; i--) 
    {
        // Shifts the i'th element one place to the right
        StoreArray0[i] = StoreArray0[i-1]; 
        StoreArray1[i] = StoreArray1[i-1];
        StoreArray2[i] = StoreArray2[i-1];
        }
    
    // Stores the latest datapoint in the first element of the array
    StoreArray0[0] = low0; 
    StoreArray1[0] = low1;
    StoreArray2[0] = low2;
    sum1 = 0.0;
    sum2 = 0.0;
    sum3 = 0.0;

    // For statement to sum the elements in the array
    for (int a = 0; a<=sizeMovAg-1; a++) 
    { 
        sum1+=StoreArray0[a];
        sum2+=StoreArray1[a];
        sum3+=StoreArray2[a];
        }
    
    // Calculates an average over the datapoints in the array    
    Average0 = sum1/sizeMovAg; 
    Average1 = sum2/sizeMovAg;
    Average2 = sum3/sizeMovAg; 
    }

// This must be applied to all emg signals coming in
void processing_emg()
{
    filtering();
    MovAg();
    }

// MOTOR_CAL
// To calibrate the motor angle to some mechanical boundaries
// Kenneth mee bezig 
void pos_store(int a){ //store position in counts to know count location of the ends of bridge

    if (tower_1_position == 0.1)
    {
        tower_1_position = a;
        }
    else if (tower_end_position == 0.1)
    {
        tower_end_position = a;
        }
    else if (rotation_start_position == 0.1)
    {
        rotation_start_position = a;
        }    
    }

// Start translation   
void translation_start(int a, float b) // a = dir , b = speed 
{ 
    dirpin_1.write(a);
    pwmpin_1 = b;
    }
    
// Stop translation                            
void translation_stop()
{ 
    pwmpin_1 = 0.0;
    }
    
// Start rotation         
void rotation_start(int a, float b)
{ 
    dirpin_2.write(a);
    pwmpin_2 = b;
    }
 
// Stop rotation    
void rotation_stop()
{ 
    pwmpin_2 = 0.0;
    } 

// Calibration of translation
void calibration_translation()
{
    for(int m = 1; m <= 2; m++) // to do each direction one time
    {
        // dir = 0, means that the pen moves to the translation motor, dir = 1 means it moves to the rotation motor
        pc.printf("\r\nTranslatie loop\r\n");
        translation_start(dir,speed); 
        pc.printf("Direction = %i\r\n", dir);
        
        bool g = true; // to make a condition for the while loop
        while (g == true)
        {
            if (button_demo == false) // if button_demo is pushed, the translation should stop and change direction
            {
                translation_stop();    
                pos_store(Counts1(counts1)); 
                pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position);
                dir = dir + 1;
                                
                g = false; // to end the while loop
                }
                
            wait(0.01); 
            }
        
        wait(1.0); // wait 3 seconds before next round of translation/rotation
        }
    }
    
void calibration_rotation()
{
    rotation_start(dir, speed);
    pc.printf("\r\nRotatie start\r\n");

    bool f = true; // condition for while loop
    while(f == true)
    {
        if (button_motorcal == false) // If button_motorcal is pushed, then the motor should stop and remain in that position until homing
        {
            rotation_stop();
            
            f = false; // to end the while loop
            }
        
        wait(0.01);
        }
    int start_counts = 0;    
    pos_store(Counts2(start_counts));
    pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position);
    }
    
void motor_calibration()
{
    // translation
    calibration_translation();
    
    pc.printf("before wait\r\n");
    wait(1.5);
    
    // rotation
    calibration_rotation();
    
    pc.printf("Motor calibration done");
    }

// WAIT
// To do nothing
void wait_mode()
{
    // Go back to the initial values
     
    // All pwm's to zero
    translation_stop();
    rotation_stop();
    // All counts to zero
    counts1 = 0;
    counts2 = 0;
    // EMG calibration to zero
    g = 0;
    dir = 0;
    }

// EMG_CAL
// To calibrate the EMG signal to some boundary values
// Void to switch between signals to calibrate
void switch_to_calibrate() 
{
    g++;
    //If g = 0, led is blue
    if (g == 0) 
    { 
        led_blue==0;
        led_red==1;
        led_green==1;
        } 
    //If g = 1, led is red
    else if(g == 1) 
    { 
        led_blue==1;
        led_red==0;
        led_green==1;
        } 
    //If g = 2, led is green
    else if(g == 2) 
    { 
        led_blue==1;
        led_red==1;
        led_green==0;
        } 
    //If g > 3, led is white
    else 
    { 
        led_blue==0;
        led_red==0;
        led_green==0;
        emg_calib = 0;
        g = 0;
        }
    }

// Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required
void calibrate() 
{
    switch (g) 
    {
        case 0: 
        { // Case zero, calibrate EMG signal of right biceps
            sum = 0.0;
            
            //For statement to make an array of the latest datapoints of the filtered signal
            for (int j = 0; j<=sizeCali-1; j++) 
            {
                
                StoreCali0[j] = low0; // Stores the latest datapoint in the first element of the array
                sum+=StoreCali0[j]; // Sums the elements in the array
                wait(0.001f);
                }
            Mean0 = sum/sizeCali; // Calculates the mean of the signal
            Threshold0 = 1.5*Mean0; // Factor *2 is for resting and *1 is for max contraction
            break;
            }
        case 1: 
        { // Case one, calibrate EMG signal of left biceps
            sum = 0.0;
            for(int j=0; j<=sizeCali-1; j++) 
            {
                StoreCali1[j] = low1;
                sum+=StoreCali1[j];
                wait(0.001f);
                }
            Mean1 = sum/sizeCali;
            Threshold1 = 2.0*Mean1; // Factor *2 is for resting and *1 is for max contraction
            break;
            }
        case 2: 
        { // Case two, calibrate EMG signal of calf
            sum = 0.0;
            for(int j=0; j<=sizeCali-1; j++) 
            {
                StoreCali2[j] = low2;
                sum+=StoreCali2[j];
                wait(0.001f);
                }   
            Mean2 = sum/sizeCali;
            Threshold2 = 2.0*Mean2; //Factor *2 is for resting and *1 is for max contraction
            break;
            }
        case 3: 
        { // Sets calibration value to 1; robot can be set to Home position
            emg_calib=1;
            wait(0.001f);
            break;
            }
        default: 
        { // Ensures nothing happens if g is not equal to 0, 1 or 2.
            break;
            }
        }
    }

// Void to calibrate the EMG signal
void emg_calibration()
{
    for(int m = 1; m <= 4; m++)
    {
        led_blue == 0;
        led_red == 1;
        led_green == 1;      
        
        pc.printf("g is %i\n\r",g);
        pc.printf("Average0 = %f , Average1 = %f, Average2 = %f \n\r",Average0, Average1, Average2);
        pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
        
        bool k = true;
        while(k == true)
        {
            if(button_motorcal == false)
            {
                calibrate(); // Calibrate threshold for 3 muscles
                k = false;
                }
            wait(0.2f); // Wait to avoid bouncing of button
            }
        
        bool h = true;
        while(h == true)
        {
            if (button_demo == false)
            {
                switch_to_calibrate(); // Switch state of calibration (which muscle)
                h = false;
                }
            
            wait(0.2f); // Wait to avoid bouncing of button
            }   
        }
        
        // Turning all leds off 
        led_red == 1;
        led_blue == 1;
        led_green == 1;
    }

// START
// To move the robot to the starting position: middle
void start_mode() 
{
    // move to middle, only motor1 has to do something, the other one you can move yourself during the calibration
    int a = tower_end_position - ((tower_end_position - tower_1_position)/2);
    pc.printf("position middle = %i, position pen = %i \r\n", a, Counts1(counts1));
    
    //translation home
    if (Counts1(counts1) > a)
    {
        translation_start(1,1.0);
        pc.printf("start to 1 \r\n");
        }
    else {    
        translation_start(0,1.0);
        pc.printf("start to 0 \r\n");
        }
    while(true){
        if ((Counts1(counts1) > (a - 500)) && (Counts1(counts1) < (a + 500)))
        {
            translation_stop();
            pc.printf("stop \r\n");
            break; 
        }
    }
}
    
// OPERATING
// To control the robot with EMG signals
// Function for using muscle for direction control    
void Directioncontrol()
{  
    switch (currentdirection)
        {
            case Pos_RB:
            
                out1 = out1;
                out2 = out2;
                directionchanged = false;
                led_red == 0;
                
                
            if (Average2 > Threshold2)
            {
                currentdirection = Pos_LB;
                pc.printf("\r\n direction = Pos_LB\r\n");
                directionchanged = true;
                }
            wait(0.5); // wait 0.5 seconds, otherwise it goes directly to the next case    
            break;    
            
            case Pos_LB:
        
                out1 = out1 * -1.0;
                out2 = out2;
                directionchanged = false;
                led_blue == 0;
                            
            if (Average2 > Threshold2)
            {
                currentdirection = Pos_RO;
                pc.printf("\r\n direction = Pos_RO\r\n");
                directionchanged = true;
                }
            wait(0.5);    
            break;
                        
            case Pos_RO:
            
                out1 = out1;
                out2 = out2 * -1.0;
                directionchanged = false;
                led_green == 0;
            
            if (Average2 > Threshold2)
            {
                currentdirection = Pos_LO;
                pc.printf("\r\n direction = Pos_LO\r\n");
                directionchanged = true;
                }
            wait(0.5);    
            break;
                
            case Pos_LO:
                out1 = out1 * -1.0;
                out2 = out2 * -1.0;
                directionchanged = false;
                led_red == 0;
                led_blue == 0;
                led_green == 0;
                
            
            if (Average2 > Threshold2)
            {
                currentdirection = Pos_RB;
                pc.printf("\r\n direction = Pos_RB\r\n");
                directionchanged = true;
                }
            wait(0.5);    
            break;
            }
    } 
    
// PID controller
// To control the input signal before it goes into the motor control
// PID execution
double PID_control(volatile double error, const double kp, const double ki, const double kd, volatile double &error_int, volatile double &error_prev)
{ 
    // P control
    double u_k = kp * error;

    // I control
    error_int = error_int + (Ts * error);
    double u_i = ki * error_int;

    // D control
    double error_deriv = (error - error_prev);
    double u_d = kd * error_deriv;
    error_prev = error;

    return u_k + u_i + u_d;
    }

void boundaries()
{
    double q2tot = q2 + dq2;
    if (q2tot > q2end)
    {
        dq2 = 0;
        } //kan ook zeggen q2end-q2 is dat dan juiste waarde of moet q2-q2end?
    else if (q2tot < q2start)
    {
        dq2 = 0;
        }
    }

void motor_control()
{
    // servocontrol(); // make sure the servo is used in this mode, maybe attach to a ticker?
    
    // filtering emg
    
    if (Average0 > Threshold0)
    {
        desired_x = 1.0;
        }
    else
    {
        desired_x = 0.0;
        }
    
    if (Average1 > Threshold1)
    {
        desired_y = 1.0;
        }
    else
    {
        desired_y = 0.0;
        }
        
    // calling functions
        
    
    // motor control
    out1 = desired_x; //control x-direction
    out2 = desired_y; //control y-direction
    Directioncontrol();
    vdesx = out1 * 20.0; //speed x-direction
    vdesy = out2 * 20.0; //speed y-direction

    q1 = Counts2(counts2) * alpha + q1start; //counts to rotation (rad) 
    q2 = Counts1(counts1)* beta + q2start; //counts to translation (mm)
    MPe = el_1 - el_2 + q2; //x location end effector, x-axis along the translation
    xe = cos(q1) * MPe; //x location in frame 0
    ye = sin(q1) * MPe; //y location in frame 0
    gamma = 1.0 /((-1.0 * ye * sin(q1)) - (xe * cos(q1))); //(1 / det(J'')inverse)
    dq1 = gamma * delta_t * (sin(q1) * vdesx - cos(q1) * vdesy); //target rotation
    dq2 = gamma * delta_t * (xe * vdesx + ye * vdesy) * -1.0; //target translation
    //boundaries();
    dC1 = PID_control( dq1, Rot_Kp, Rot_Ki, Rot_Kd, Rot_error, Rot_prev_error) / alpha; //target rotation to counts
    dC2 = PID_control( dq2, Trans_Kp, Trans_Ki, Trans_Kd, Trans_error, Trans_prev_error) / beta; //target translation to counts
    pwm1 = 3.0 * (dC1 / delta_t) / 8400.0; //
    pwm2 = 7.0 * (dC2 / delta_t) / 8400.0; //
    dirpin_1.write(pwm2 < 0); // translatie                 
    pwmpin_1 = fabs (pwm2);                    
    dirpin_2.write(pwm1 < 0); // rotatie
    pwmpin_2 = fabs (pwm1);
    }   

// DEMO
// To control the robot with a written code and write 'HELLO'
// Voor het laatst bewaren
void demo_mode()
{
    // code here
    }

// FAILURE
// To shut down the robot after an error etc 
void failure_mode()
{
    if (button_emergency == false) // condition for MOTOR_CAL --> FAILURE; button_emergency press 
    {    
        led_red == 0; // turning red led on to show emergency mode 

        // all pwmpins zero 
        pwmpin_1 = 0.0;
        pwmpin_2 = 0.0;
    
        // Servo up?
        // myservo = 0.1;
    
        pc.printf("\r\nEmergency mode, reset system to continue\r\n");
        }
    }

// Main function
// -----------------------------------------------------------------------------
int main()
{
    pc.baud(115200); // For TeraTerm, the baudrate, set also in TeraTerm itself! 
    pc.printf("Start code\r\n"); // To check if the program starts 
    pwmpin_1.period_us(60); // Setting period for PWM
    process_tick.attach(&processing_emg,T); // EMG signals must be filtered all the time! EMG signals filtered every T sec.
    emergency.attach(&failure_mode,0.01); // To make sure you can go to emergency mode all the time
    
    while(true){
        // timer
        clock_t start; // start the timer
        start = clock(); 
        time_overall = (clock() - start) / (double) CLOCKS_PER_SEC;
        myservo = 0.1; // Keep the pen lifted until servo function is called (operation mode)
        
        //pc.printf("potmeter value = %f ", potmeter_value);
        //pc.printf("counts = %i\r\n", counts); 
        
        // With the help of a switch loop and states we can switch between states and the robot knows what to do
        switch(CurrentState)
        {
            case WAIT:
            
            StateChanged = true;
            pc.printf("\r\nState is WAIT\r\n");
            
            if(StateChanged) // so if StateChanged is true
            {
                // Execute WAIT mode
                wait_mode();
                
                StateChanged = false; // the state is still WAIT
                }
                
            if(button_motorcal == false) // condition for WAIT --> MOTOR_CAl; button_motorcal press
            {
                CurrentState = MOTOR_CAL;
                pc.printf("\r\nState is MOTOR_CAL\r\n");
                StateChanged = true;
                }
            
            break;
            
            case MOTOR_CAL:
            
            if(StateChanged) // so if StateChanged is true
            {
                // Execute MOTOR_CAL mode
                motor_calibration();
                
                StateChanged = false; // the state is still MOTOR_CAL
                }
            
            if((pwmpin_1 < 0.01) && (pwmpin_2 < 0.01)) // condition for MOTOR_CAL --> EMG_CAL; 3s and motors stopped moving
            {
                CurrentState = EMG_CAL;
                pc.printf("\r\nState is EMG_CAL\r\n");
                StateChanged = true;
                }
               
            break;
            
            case EMG_CAL:
            
            if(StateChanged) // so if StateChanged is true
            {
                // Execute EMG_CAL mode
                emg_calibration();
                
                StateChanged = false; // state is still EMG_CAL
                }
            
            if((Average0 < 0.04) && (Average1 < 0.04) && (Average2 < 0.04)) // condition for EMG_CAL --> START; 5s and EMG is low
            {
                CurrentState = START;
                pc.printf("\r\nState is START\r\n");
                StateChanged = true;
                }
                
            break;
            
            case START:
            
            if(StateChanged) // so if StateChanged is true
            {
                // Execute START mode
                start_mode();
                
                pc.printf("pwmpin_1 = %f pwmpin_2 = %f \r\n", pwmpin_1, pwmpin_2);
                
                StateChanged = false; // state is still START
                }
            
            if((pwmpin_1 < 0.01) && (pwmpin_2 < 0.01)) // condition for START --> OPERATING; 5s and motors stopped moving
            {
                CurrentState = OPERATING;
                pc.printf("\r\nState is OPERATING\r\n");
                StateChanged = true;
                }
                   
            break;
            
            case OPERATING:
            
            if(StateChanged) // so if StateChanged is true
            {
                // Execute OPERATING mode
                bool g = true;
                while(g == true)
                {
                    motor_control();
                    pc.printf("PWM_rot = %f PWM_trans = %f VdesX = %f VdesY = %f \n\r",pwm1,pwm2,vdesx,vdesy);
                    pc.printf("out1 = %f out2 = %f \n\r", out1, out2);
                    //pc.printf("Average0 = %f Average1 = %f Average2 = %f\r\n", Average0,Average1,Average2);
                
                    if(button_wait == false) // condition OPERATING --> WAIT; button_wait press  
                    {
                        CurrentState = WAIT;
                        pc.printf("\r\nState is WAIT\r\n");
                        g = false;
                        break;
                        }
                    
                    wait(delta_t);
                    }
                    
                StateChanged = false; // state is still OPERATING
                }
            
            break;
        
            // no default  
            }
            
        // while loop does not have to loop every time
        }
        
}