File content as of revision 9:c19f6f0f5080:

#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

/*
// ENCODER
QEI Encoder(D12,D13,NC,32);                                                     // CONNECT ENC2A TO D13, ENC2B TO D12
float vorig_omwentelingen = 0;
float degrees;
float vorig_degrees = 0;
float omwentelingen;
float counts;
float snelheid_tpm;
float snelheid_degps;
float snelheid_tps;
*/

// PINS
DigitalOut motor1DirectionPin(D4);                                              // Value 0: CCW; 1: CW
PwmOut motor1MagnitudePin(D5);
DigitalOut motor2DirectionPin(D7);                                              // Value 0: CW or CCW?; 1: CW or CCW?
PwmOut motor2MagnitudePin(D6);
InterruptIn button1(D2);                                                        // CONNECT BUT1 TO D2
InterruptIn button2(D3);                                                        // CONNECT BUT2 TO D3
InterruptIn button3(SW2);
InterruptIn button4(SW3);
AnalogIn pot1Pin(A0);
AnalogIn pot2Pin(A1);

// DEFINITIONS
const float motorVelocity = 4.2;                                                  // unit: rad/s
const float motorGain = 8.4;                                                    // unit: (rad/s) / PWM
const int motorRatio = 131;                                                     // Ratio of the gearbox in the motors
int motor1State = 0;                                                            // 0: Off, 1: CW, 2: CCW
int motor2State = 0;
float motorValue = 0;

//TICKERS
Ticker encoder;
Ticker checkMotor1State;
Ticker checkMotor2State;

// FUNCTIONS
void RotateMotor1CW()
{
    motor1DirectionPin = 1;
    //motor1MagnitudePin = fabs(motorVelocity / motorGain);
    motorValue = pot1Pin;
    motor1MagnitudePin = motorValue;
    //pc.printf("Rotating motor 1 CW \r\n");
    pc.printf("Motor 1 CW %f \r\n", motorValue);   
}

void RotateMotor1CCW()
{
    motor1DirectionPin = 0;
    //motor1MagnitudePin = fabs(motorVelocity / motorGain);
    //pc.printf("Rotating motor 1 CounterCW \r\n");
    motorValue = pot1Pin;
    motor1MagnitudePin = motorValue;
    //pc.printf("Rotating motor 1 CW \r\n");
    pc.printf("Motor 1 CCW %f \r\n", motorValue);   
}

void TurnMotor1Off()
{
    motor1MagnitudePin = 0;
    pc.printf("Motor 1 off \r\n");
}

void CheckMotor1()                                                               // Checks the state of motor1 and rotates motor1 depending on the state
{
    switch(motor1State)
    {
        case 0:                                                                 // Turn motors off
        {TurnMotor1Off(); break;}
        
        case 1:                                                                 // Rotate motor 1 CW
        {RotateMotor1CW(); break;}
        
        case 2:                                                                 // Rotate motor 2 CCW
        {RotateMotor1CCW(); break;}
        
        default:
        break;
    }
}

void RotateMotor2CW()
{
    motor2DirectionPin = 1;
    //motor1MagnitudePin = fabs(motorVelocity / motorGain);
    motorValue = pot2Pin;
    motor2MagnitudePin = motorValue;
    //pc.printf("Rotating motor 1 CW \r\n");
    pc.printf("Motor 2 CW %f \r\n", motorValue);   
}

void RotateMotor2CCW()
{
    motor2DirectionPin = 0;
    //motor1MagnitudePin = fabs(motorVelocity / motorGain);
    //pc.printf("Rotating motor 1 CounterCW \r\n");
    motorValue = pot2Pin;
    motor2MagnitudePin = motorValue;
    //pc.printf("Rotating motor 1 CW \r\n");
    pc.printf("Motor 2 CCW %f \r\n", motorValue);   
}

void TurnMotor2Off()
{
    motor2MagnitudePin = 0;
    pc.printf("Motor 2 off \r\n");
}

void CheckMotor2()                                                               // Checks the state of motor1 and rotates motor1 depending on the state
{
    switch(motor2State)
    {
        case 0:                                                                 // Turn motors off
        {TurnMotor2Off(); break;}
        
        case 1:                                                                 // Rotate motor 1 CW
        {RotateMotor2CW(); break;}
        
        case 2:                                                                 // Rotate motor 2 CCW
        {RotateMotor2CCW(); break;}
        
        default:
        break;
    }
}
/*
void EncoderCalc()
{
    counts = Encoder.getPulses()/motorRatio;
    degrees = counts/32*360;
    omwentelingen = counts/32;
    snelheid_tpm = (omwentelingen-vorig_omwentelingen)/0.5*60;
    snelheid_tps = (omwentelingen-vorig_omwentelingen)/0.5;
    snelheid_degps = (degrees-vorig_degrees)/0.5;
    vorig_omwentelingen = omwentelingen;
    vorig_degrees = degrees;
    //pc.printf("%.1f pulsen, %.2f graden, %.2f omwentelingen, %.2f tpm, %.2f tps, %.2f deg/s\r\n",counts, degrees, omwentelingen, snelheid_tpm, snelheid_tps, snelheid_degps);
}
*/

// States function
void ProcessStateMachine()
{
    switch(currentState)
    {
        case MOTORS_OFF:
        {
            // State initialization
            if(stateChanged)
            {
                pc.printf("Entering MOTORS_OFF \r\n");
                TurnMotor1Off();                                                // Turn motors off
                stateChanged = false;
            }
            
            // Home command
            if(!button1)
            {
                currentState = MOVING;
                stateChanged = true;
                break;
            }
            break;
        }
        
        case MOVING:
        {
            // State initialization
            if(stateChanged)
            {
                pc.printf("Entering MOVING \r\n");
                stateChanged = false;
            }

            // Move commands
            
            if(!button1){motor1State = 1;}
            else if(!button2){motor1State = 2;}
            else{motor1State = 0;}
            
            if(!button3){motor2State = 1;}
            else if(!button4){motor2State = 2;}
            else{motor2State = 0;}
            
            /*
            // Hit command    
            if(// HIT COMMAND)
            {
                currentState = HITTING;
                stateChanged = true;
                break;
            }
            */
            break;
        }
        
        case HITTING:
        {
            // State initialization
            if(stateChanged)
            {
                pc.printf("Entering HITTING \r\n");
                //HitBall();                                                    // Hit the ball
                stateChanged = false;
                currentState = MOVING;
                stateChanged = true;
                break;
            }
            break;
        }
        
        default:
        {
            TurnMotor1Off();                                                    // Turn motors off for safety
            TurnMotor2Off();
            break;
        }
    }
}

// Main function
int main()
{
    // Serial communication
    pc.baud(115200);
    
    pc.printf("START \r\n");
    
    //encoder.attach(EncoderCalc, 0.5);
    checkMotor1State.attach(&CheckMotor1, 0.01);
    checkMotor2State.attach(&CheckMotor2, 0.01);
    
    while(true)
    {
        ProcessStateMachine();                                                  // Execute states function
    }
}