File content as of revision 1:1221419474b3:

// TEST VERSION
#include "BiQuad.h"
#include "FastPWM.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "mbed.h"
#include "QEI.h"

// SERIAL COMMUNICATION WITH PC
MODSERIAL pc(USBTX, USBRX);

// STATES
enum states{MOTORS_OFF, HOMING, MOVING, HITTING};

states currentState = MOTORS_OFF;                                               // Start with motors off
bool stateChanged = true;                                                       // Make sure the initialization of first state is executed

// DEFINITIONS
InterruptIn homeButton(SW2);                                                    // Button to go to home state
InterruptIn hitButton(SW3);                                                     // Button to go to hit state

float voltage;

// FUNCTIONS
// Turn motors off
void TurnMotorsOff()
{
    pc.printf("Turning motors off \r\n");                                       // Turn motors off
}

// Move to home
void MoveToHome()
{
    pc.printf("Moving to home \r\n");                                           // Move to home
}

// Filter signals
float FilterSignal(float signal)
{
    pc.printf("Filtering signal \r\n");                                         // Filter signal
    voltage = signal;
    return voltage;
}

// Motor 1
void RotateMotor1(float voltage)
{
    pc.printf("Rotating motor 1 \r\n");                                         // Rotate motor 1
}

// Motor 2
void RotateMotor2(float voltage)
{
    pc.printf("Rotating motor 2 \r\n");                                         // Rotate motor 2
}

// Hit the ball
void HitBall()
{
    pc.printf("Hitting the ball \r\n");                                         // Rotate motor 3
}

// States function
void ProcessStateMachine()
{
    switch(currentState)
    {
        case MOTORS_OFF:
        {
            // State initialization
            if(stateChanged)
            {
                pc.printf("Entering MOTORS_OFF \r\n");
                TurnMotorsOff();                                                // Turn motors off
                stateChanged = false;
            }
            
            // Home command
            if(!homeButton)
            {
                currentState = HOMING;
                stateChanged = true;
                break;
            }
        }
        
        case HOMING:
        {
            // State initialization
            if(stateChanged)
            {
                pc.printf("Entering HOMING \r\n");
                MoveToHome();                                                   // Move to home position
                stateChanged = false;
                currentState = MOVING;
                stateChanged = true;
                break;
            }
        }
        
        case MOVING:
        {
            // State initialization
            if(stateChanged)
            {
                pc.printf("Entering MOVING \r\n");
                stateChanged = false;
            }
            
            // EMG signals to rotate motor 1
            if(false)
            {
                FilterSignal(1);                                                // Filter the signal
                RotateMotor1(voltage);                                          // Rotate motor 1
                pc.printf("%f \r\n", voltage);
            }
            
            // EMG signals to rotate motor 2
            if(false)
            {
                FilterSignal(2);                                                // Filter the signal
                RotateMotor2(voltage);                                          // Rotate motor 2
                pc.printf("%f \r\n", voltage);
            }
            
            // Hit command    
            if(!hitButton)
            {
                currentState = HITTING;
                stateChanged = true;
                break;
            }
        }
        
        case HITTING:
        {
            // State initialization
            if(stateChanged)
            {
                pc.printf("Entering HITTING \r\n");
                HitBall();                                                      // Hit the ball
                stateChanged = false;
                currentState = MOVING;
                stateChanged = true;
                break;
            }
        }
        
        default:
        {
            TurnMotorsOff();                                                    // Turn motors off for safety
        }
    }
}

// Main function
int main()
{
    // Serial communication
    pc.baud(115200);
    
    while(true)
    {
        ProcessStateMachine();                                                  // Execute states function
    }
}