File content as of revision 3:5c3edcd29448:

#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, MOVING, HITTING};

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

// PINS
DigitalOut motor1DirectionPin(D4);                                              // Value 0: CW or CCW?; 1: CW or CCW?
PwmOut motor1MagnitudePin(D5);
DigitalOut motor2DirectionPin(D7);                                              // Value 0: CW or CCW?; 1: CW or CCW?
PwmOut motor2MagnitudePin(D6);
InterruptIn button1(D2);                                                        // BUTTON 1 AANSLUITEN OP D2!!!
InterruptIn button2(D3);                                                        // BUTTON 2 AANSLUITEN OP D3!!!

// DEFINITIONS
const float motorVelocity = 4;                                                  // rad/s
const float motorGain = 4;                                                      // (rad/s) / PWM

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

void TurnMotor1CW()
{
    motor1DirectionPin = 0;
    motor1MagnitudePin = fabs(motorVelocity / motorGain);   
}

void TurnMotor1CCW()
{
    motor1DirectionPin = 1;
    motor1MagnitudePin = fabs(motorVelocity / motorGain);   
}

void TurnMotorsOff()
{
    motor1MagnitudePin = 0;
}

// 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(button1)
            {
                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;
            }
            */
            // Motor testen
            while(button1)
            {
                TurnMotor1CW();
                pc.printf("Turning motor 1 CW \r\n");
            }
            
            while(button2)
            {
                TurnMotor1CCW();
                pc.printf("Turning motor 1 CounterCW \r\n");
            }
            
            TurnMotorsOff();
        }
        
        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
    }
}