File content as of revision 2:5730195cf595:

// Robot states
#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"

// Define objects
MODSERIAL pc(USBTX,USBRX);
DigitalOut led_red(LED_RED);
DigitalOut led_blue(LED_BLUE);
DigitalOut led_green(LED_GREEN);
InterruptIn button_Mbed(PTC6); //Button 1 Mbed
InterruptIn button_1(PTB10); //Button 2 BRS
InterruptIn button_2(PTB11); // Button 3 BRS

// Functions 
enum states{START,KAL_ME,KAL_EMG,MOVE_START,READY_START,DEMO,MOVE,WAIT,OFF};

states CurrentState = START;
bool StateChanged = true;         // this is the initialization of the first state

// Function START_TO_KAL_ME

void StateMachine(void)
{
    switch(CurrentState)
    {
        case START:     
            if (StateChanged)
            {
                pc.printf("Start state, red led is on. If button 1 pressed, go to kal_me state");
                led_red = 0;  // Red led is on 
                led_blue = 1;
                led_green = 1;
                StateChanged = false;
            }
            if (button_Mbed.mode (PullDown)== false; ) // State switches when button pressed
            {
                CurrentState = KAL_ME;
                StateChanged = true;
                wait(0.2f);
                
            }
        break;      // end of state START
                
        case KAL_ME:
            if (StateChanged)
            {
                pc.printf("Calibration ME state, red ld flickers slow"); 
                //FUNCTION Red led flickers slow
                
                
                // FUNCTION Move to mechanical stop, include v_motor, t_passed
                // FUNCTION Reset encoders
                
                StateChanged = false;
            }
            
            if  (v_motor == 0 && t_passed > 2) // FUNCTION t_passed, included in v_motor?
            {
                CurrentState = KAL_EMG;
                StateChanged = true;
            }
        break;      // end of state KAL_ME
        
        case KAL_EMG:
            if (StateChanged)
            {
                // FUCNTION Red led flickers fast
                
                //FUNCTION Measure EMG_max, EMG variable meten, t_passed
                
                StateChanged = false;
            }
            
            if (EMG < 0.1*EMG_max && t_passed > 2)
            {
                CurrentState = MOVE_START;
                StateChanged = true;
            }
        break;      // end of state KAL_EMG
        
        case MOVE_START:
            if(StateChanged)
            {
                led_red = 1;
                led_blue = 1; 
                led_green = 0;   // Green led is on
            
                // FUNCTION move to start, t_passed
                // Define current_position & start_position 
                StateChanged = false; 
            }
            
            if (current_position == start_position && t_passed > 2) // FUNCTIO t_passed
            {
                CurrentState = READY_START;
                StateChanged = true;
            } 
        break;      // end of state MOVE_START
        
        case READY_START:
            if (StateChanged)
            {
                led_red = 1;
                led_blue = 1;
                led_green = 0;       // Green led is on 
                
                StateChanged = false;
            }
            
            if (button_1.read() == false)   // Button 1
            {
                CurrentState = DEMO;
                StateChanged = true;
                wait(0.2f);
            }
            else if (button_2.read() == false || EMG > 0.2*EMG_max) // Button 2 or 20% EMG_max
            {
                CurrentState = MOVE;
                StateChanged = true;
                wait(0.2f);
            }                
        break;      // end of state READY_START
        
        case DEMO:
            if (StateChanged)
            {
                //FUNCTION Blue led blink fast
                
                //FUNCTION perform straight movements for demo
                
                StateChanged = false; 
            }
            
            if (current_position == end_position && t_passed > 2) 
            {
                CurrentState = MOVE_START;
                StateChanged = true;
            }
        break;      // end of state DEMO
        
        case MOVE:
            if (StateChanged)
            {
                led_red = 1;
                led_green = 1;
                led_blue = 0;  //Blue led is on 
                
                // FUNCTION Play the game with EMG signal
                
                StateChanged = false; 
            }
            
            if (button_Mbed.read() == false)
            {
                CurrentState = OFF;
                StateChanged = true;
                wait(0.2f);
            }
            
            else if (button_1.read() == false || EMG < 0.2*EMG_max) 
            {
                CurrentState = WAIT;
                StateChanged = true;
                wait(0.2f);
            }
            break;      // end of state MOVE
        
        case WAIT:
            if (StateChanged)
            {
                led_red = 0;   // Pink led iS on 
                led_blue = 0; 
                led_green = 1
                
            if (button_Mbed.read() == false) // For five second, too hard
            {
                CurrentState = OFF;
                StateChanged = true;
                wait(0.2f);
            )
            else if (button_1.read() == false) 
            {
                CurrentState = MOVE_START;
                StateChanged = true;
                wait(0.2f);
            }
            else if (button_2.read() == false || EMG > 0.2*EMG_max) 
            {
                CurrentState = MOVE;
                StateChanged = true;
                wait(0.2f);
            }
            
        break;      // end of state WAIT
        
        case OFF:
            led_red = 0;       // White led is on 
            led_blue = 0; 
            led_green = 0;
            
        break;      // end of state OFF
        
        default:
        TurnMotorsoff(); //FUNCTION 
        printf("Unknown state reached");
    }               // End of the switch, all states are prescribed
}

int main(void)      // wat hier in moet snap ik nog niet 
{
        // hier moeten dingen komen
        while (true)
        {
            CheckForCommandFromTerminal();
            StateMachine();
        }
}