File content as of revision 2:d3687b2c4e37:

#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


// FUNCTIONS
// Turn motors off
void TurnMotorsOff()
{
    // Turn motors off
}

// Move to home
void MoveToHome()
{
    // Move to home
}

// Filter signals
float FilterSignal(// Signal)
{
    // Filter signal
    return // Voltage
}

// Motor 1
void RotateMotor1(// Voltage)
{
    // Rotate motor 1
}

// Motor 2
void RotateMotor2(// Voltage)
{
    // Rotate motor 2
}

// Hit the ball
void HitBall()
{
    // 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(//HOME COMMAND)
            {
                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(// EMG signal)
            {
                FilterSignal(// Signal);                                        // Filter the signal
                RotateMotor1(// Voltage);                                       // Rotate motor 1
            }
            
            // EMG signals to rotate motor 2
            if(// EMG signal)
            {
                FilterSignal(// Signal);                                        // Filter the signal
                RotateMotor2(// Voltage);                                       // Rotate motor 2
            }
            
            // Hit command    
            if(// HIT COMMAND)
            {
                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
    }
}