File content as of revision 9:e8cc37a94fec:

#include "mbed.h"
#include "HIDScope.h"
#include "BiQuad.h"
#include "MODSERIAL.h"
#include "FastPWM.h"
#include "QEI.h"
#include <math.h>

#include "Motor_Control.h"

DigitalIn button1(D12);
InterruptIn button2(SW2);
DigitalOut ledr(LED_RED);

AnalogIn shield0(A0);
AnalogIn shield1(A1);
AnalogIn shield2(A2);
AnalogIn shield3(A3);

Ticker measurecontrol;
DigitalOut motor1Direction(D7);
FastPWM motor1Velocity(D6);
MODSERIAL pc(USBTX, USBRX);
QEI Encoder(D8,D9,NC,8400);

float PI = 3.1415926;//535897932384626433832795028841971693993;
float timeinterval = 0.001;         // Time interval of the Ticker function
bool whileloop = true;              // Statement to keep the whileloop in the main function running
                                    // While loop has to stop running when failuremode is activated

// Define the different states in which the robot can be
enum States {MOTORS_OFF, EMG_CALIBRATION, MOTOR_CALIBRATION,
    START_GAME, DEMO_MODE, GAME_MODE, MOVE_END_EFFECTOR,
    MOVE_GRIPPER, END_GAME, FAILURE_MODE};

// Default state is the state in which the motors are turned off
States MyState = MOTORS_OFF;



void motorsoff() {
    bool aa = true;                                 // Boolean for the while loop
    sendtomotor(0);
    while (aa) {
        if (button1.read() == 0) {                  // If button1 is pressed, set MyState to EMG_CALIBRATION and exit the while loop
            MyState = EMG_CALIBRATION;
            aa = false;
        }
    }
}


//P control implementation (behaves like a spring)
double P_controller(double error)
{
    double Kp = 17.5;
    //Proportional part:
    double u_k = Kp * error;
    
    //sum all parts and return it
    return u_k;
}

void nothing(){// Do nothing
}


void New_State() {
    switch (MyState)
    {
        case MOTORS_OFF :
            pc.printf("State: Motors turned off");
            motorsoff();
            break;
        
        case EMG_CALIBRATION :
            pc.printf("State: EMG Calibration");
            break;
        
        case MOTOR_CALIBRATION :
            pc.printf("State: Motor Calibration");
            break;
        
        case START_GAME :
            pc.printf("State: Start game");
            break;
        
        case DEMO_MODE :
            pc.printf("State: Demo mode");
            break;
        
        case GAME_MODE :
            pc.printf("State: Game mode");
            break;
        
        case MOVE_END_EFFECTOR :
            pc.printf("State: Move end effector");
            break;
        
        case MOVE_GRIPPER :
            pc.printf("State: Move the gripper");
            break;
        
        case END_GAME :
            pc.printf("State: End of the game");
            break;
        
        case FAILURE_MODE :
            pc.printf("FAILURE MODE!!");            // Let the user know it is in failure mode
            ledr = 0;                               // Turn red led on to show that failure mode is active
            whileloop = false;
            break;
        
        default :
            pc.printf("Default state: Motors are turned off");
            sendtomotor(0);
            break;
    }
}

void failuremode() {
    MyState = FAILURE_MODE;
    New_State();
}

int main()
{
    pc.printf("Starting...\r\n\r\n");
    double frequency = 17000.0;
    double period_signal = 1.0/frequency;
    pc.baud(115200);
    
    button2.fall(failuremode);          // Function is always activated when the button is pressed
    motor1Velocity.period(period_signal);
    measurecontrol.attach(measureandcontrol, timeinterval);
    
    int previous_state_int = (int)MyState;
    New_State();
    
    while(whileloop)
    {
        if ( (previous_state_int - (int)MyState) != 0 ) {
            New_State();
        }
        previous_state_int = (int)MyState;
    }
    
    pc.printf("Programm stops running\r\n");
    sendtomotor(0);
    measurecontrol.attach(nothing,10000);
    return 0;
}