File content as of revision 2:5cace74299e7:

#include "mbed.h"
#include "FastPWM.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"

enum States {WaitState, MotorCalState, EMGCalState, HomingState, MovingState, GrippingState, ScrewingState, FailureState};
States currentState = WaitState;

DigitalIn startButton(D0);
InterruptIn failureButton(D1);
DigitalIn grippingSwitch(SW2);
DigitalIn screwingSwitch(SW3);
DigitalIn gripDirection(D2);
DigitalIn screwDirection(D3);
MODSERIAL pc(USBTX, USBRX);
DigitalOut led1(LED1); // Red led
DigitalOut led2(LED2); // Green led
DigitalOut led3(LED3); // Blue led
QEI encoder1(D14, D15, NC, 8400, QEI::X4_ENCODING);
//QEI encoder2(A2, A3), NC, 4200);
//QEI encoder3(A4, A5), NC, 4200);
AnalogIn pot(A0); // Speed knob

bool stateChanged = true;

Ticker mainTicker;
Timer stateTimer;

const double sampleTime = 0.001;
const float maxVelocity=8.4; // in rad/s
const double PI = 3.141592653589793238463;
                                    
double u1, u2, u3, u4; // u1 is motor output of the long link, u2 is motor of the short link, u3 is motor of gripper, u4 is motor of screwer
FastPWM pwmpin1(D5); //motor pwm
DigitalOut directionpin1(D4); // motor direction
double robotEndPoint;

double v;
double Dpulses;

void switchToFailureState ()
{
    failureButton.fall(NULL); // Detaches button, so it can only be activated once
    led1 = 0;
    led2 = 1;
    led3 = 1;
    pc.printf("SYSTEM FAILED\n");
    currentState = FailureState;
    stateChanged = true;
}

double measureVelocity (int motor)
{
    static double lastPulses = 0;
    double currentPulses;
    static double velocity = 0;
    
    static int i = 0;
    if (i == 10) { // Encoder is not accurate enough, so with 1000 Hz the velocity can only be 0, 1000, 2000 or 3000 pulses/s
    switch (motor) { // Check which motor to measure
        case 1:
            currentPulses = encoder1.getPulses();
            break;
        case 2:
            //currentPulses = encoder2.getPulses();
            break;
        case 3:
            //currentPulses = encoder3.getPulses();
            break;
    }

    double deltaPulses = currentPulses - lastPulses;
    Dpulses = deltaPulses;
    velocity = deltaPulses / (sampleTime * 10); // Velocity in pulses/s
    lastPulses = currentPulses;
    i = 0;
    } else {
        i += 1;
    }
    v = velocity;
    return velocity;
}

void measureAll ()
{

}

void getMotorControlSignal ()   // Milestone 1 code, not relevant anymore
{
    double potSignal = pot.read(); // read pot and scale to motor control signal
    //pc.printf("motor control signal = %f\n", potSignal);
    u1 = potSignal;
}

void controlMotor ()   // Control direction and speed
{
    directionpin1 = u1 > 0.0f; // Either true or false
    pwmpin1 = fabs(u1);
}

void stateMachine ()
{
    switch (currentState) {
        case WaitState:
            if (stateChanged == true) {
                led1 = 0;
                led2 = 1;
                led3 = 1;
                // Entry action: all the things you do once in this state
                u1 = 0; // Turn off all motors
                u2 = 0;
                u3 = 0;
                u4 = 0;
                stateChanged = false;
            }

            if (startButton.read() == false) { // When button is pressed, value is false
                //pc.printf("Switching to motor calibration");
                led1 = 1;
                currentState = MotorCalState;
                stateChanged = true;
            }

            break;
        case MotorCalState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state
                led2 = 0;
                // Set motorpwm to 'low' value
                u1 = 0.6; //TODO: Check if direction is right
                u2 = 0.6;
                stateTimer.reset();
                stateTimer.start();
                stateChanged = false;
            }

            // Add stuff you do every loop
            getMotorControlSignal();

            if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && screwingSwitch.read() == false) { //TODO: add && fabs(measureVelocity(2)) < 0.1f 
                //TODO: Add reset of encoder2
                led2 = 1;
                encoder1.reset(); // Reset encoder for the 0 position
                currentState = EMGCalState;
                stateChanged = true;
                u1 = 0; // Turn off motors
                u2 = 0;
            }
            break;
        case EMGCalState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led3 = 0;
                stateTimer.reset();
                stateTimer.start();
                stateChanged = false;
            }

            // Add stuff you do every loop

            if (stateTimer >= 3.0f) {
                //pc.printf("Starting homing...\n");
                led3 = 1;
                currentState = HomingState;
                stateChanged = true;
            }
            break;
        case HomingState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led1 = 0;
                led2 = 0; // Emits yellow together
                //TODO: Set intended position
                stateChanged = false;
            }

            // Nothing extra happens till robot reaches starting position and button is pressed

            if (startButton.read() == false) { //TODO: Also add position condition
                led1 = 1;
                led2 = 1;
                currentState = MovingState;
                stateChanged = true;
            }
            break;
        case MovingState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led1 = 0;
                led2 = 0;
                led3 = 0; // Emits white together
                stateChanged = false;
            }

            if (grippingSwitch.read() == false) {
                led1 = 1;
                led2 = 1;
                led3 = 1;
                currentState = GrippingState;
                stateChanged = true;
            }

            break;
        case GrippingState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led2 = 0;
                led3 = 0; // Emits light blue together
                stateChanged = false;
            }

            if (gripDirection == true) {
                // Close gripper
            } else {
                // Open gripper
            }

            if (screwingSwitch.read() == false) {
                led2 = 1;
                led3 = 1;
                currentState = ScrewingState;
                stateChanged = true;
            }
            if (startButton.read() == false) {
                led2 = 1;
                led3 = 1;
                currentState = MovingState;
                stateChanged = true;
            }
            break;
        case ScrewingState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state;
                led1 = 0;
                led3 = 0; // Emits pink together
                stateChanged = false;
            }

            if (screwDirection == true) {
                // Screw
            } else {
                // Unscrew
            }

            if (startButton.read() == false) {
                led1 = 1;
                led3 = 1;
                currentState = MovingState;
                stateChanged = true;
            }
            break;
        case FailureState:
            if (stateChanged == true) {
                // Entry action: all the things you do once in this state
                u1 = 0; // Turn off all motors
                u2 = 0;
                u3 = 0;
                u4 = 0;
                stateChanged = false;
            }

            static double blinkTimer = 0;
            if (blinkTimer >= 0.5) {
                led1 = !led1;
                blinkTimer = 0;
            }
            blinkTimer += sampleTime;

            break;
    }
}

void mainLoop ()
{
    // Add measure, motor controller and output function
    measureAll();
    stateMachine();
    controlMotor();
}

int main()
{
    pc.printf("checkpoint 1\n");
    pc.baud(115200);
    mainTicker.attach(mainLoop, sampleTime);
    failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated

    while (true) {
        //pc.printf("State = %i\n", currentState);
        //int pulses = encoder1.getPulses();
        //pc.printf("pulses = %i\n", pulses);
        pc.printf("v = %f\n", v);
        pc.printf("delta pulses = %f\n", Dpulses);
        wait(1);
    }
}