File content as of revision 22:cce4dc5738af:

// MOTOR_CONTROL FUNCTION HAS TO BE ADJUSTED TO TWO MOTORS
// reference velocity has to be fixed? idk? --> wait for file from bram en paul
// Coefficients for the filters have to be adjusted --> Is 1 filter enough?

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

#include <cmath>
#include <deque>

#include "Motor_Control.h"

DigitalIn button1(D13);     // Button1 input to go to next state
InterruptIn button2(SW2);   // Button2 input to activate failuremode()
InterruptIn button3(SW3);   // Button3 input to go to a specific state
DigitalOut ledg(LED_GREEN); // to test stuff
DigitalOut ledr(LED_RED);   // Red LED output to show

AnalogIn S0(A0), S1(A1), S2(A2),S3(A3);    // Input EMG Shield 0,1,2,3
DigitalOut  MD0(D7), MD1(D4); // MotorDirection of motors 0,1,2
FastPWM MV0(D6), MV1(D5), MV2(D12); // MotorVelocities of motors 0,1,2
QEI E0(D8,D9,NC,8400), E1(D10,D11,NC,8400); // Encoders of motors 0,1,2

Ticker measurecontrol;              // Ticker function for motor in- and output

// Make arrays for the different variables for the motors
AnalogIn Shields[4] = {S0, S1, S2, S3};
DigitalOut MotorDirections[2] = {MD0, MD1};
FastPWM MotorVelocities[3] = {MV0, MV1, MV2};
QEI Encoders[2] = {E0, E1};

Serial pc(USBTX, USBRX);

float PI = 3.1415926f;//535897932384626433832795028841971693993;
float timeinterval = 0.001f;        // 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, OPERATING_MODE, MOVE_GRIPPER, END_GAME, FAILURE_MODE
            };


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

// Initialise the functions

void motorsoff();

void measure_data(double &rms_0, double &rms_1, double &rms_2, double &rms_3);

void det_max(double y, double &max_y);

void emgcalibration();

double P_controller(double error);

void nothing()
{
    // Do nothing
}

void get_input(char c);

void startgame() ;

void operating_mode();

void New_State();

void Set_State();

void failuremode();


//__________________________________________________________________________________________________________________________________
//__________________________________________________________________________________________________________________________________
int main()
{
    pc.baud(115200);
    ledr = 1;
    ledg = 1;

    pc.printf("Starting...\r\n\r\n");
    double frequency = 17000.0;            // Set motorfrequency
    double period_signal = 1.0/frequency;  // Convert to period of the signal
    pc.baud(115200);

    button2.fall(failuremode);              // Function is always activated when the button is pressed
    button3.fall(Set_State);                // Function is always activated when the button is pressed

    for (int i = 0; i < 3; i++) {
        MotorVelocities[i].period(period_signal);   // Set the period of the PWMfunction
    }

    measurecontrol.attach(measureandcontrol, timeinterval); // Ticker function to measure motor input and control the motors

    int previous_state_int = (int)MyState;  // Save previous state to compare with current state and possibly execute New_State()
    // in the while loop

    New_State();                            // Execute the functions belonging to the current state

    while(whileloop) {
        if ( (previous_state_int - (int)MyState) != 0 ) {   // If current state is not previous state execute New_State()
            New_State();
        }
        previous_state_int = (int)MyState;                  // Change previous state to current state
    }
    // While has stopped running
    pc.printf("Programm stops running\r\n");    // So show that the programm is quiting
    sendtomotor(0.0);                          // Set the motor velocity to 0
    measurecontrol.attach(nothing,10000);       // Attach empty function to Ticker (?? Appropriate solution ??)

    return 0;
}
//__________________________________________________________________________________________________________________________________
//__________________________________________________________________________________________________________________________________

void motorsoff()
{
    // Function to turn the motors off. First state that the robot has. Robot will stay in this state untill button1 is pressed.
    // Robot will not return to this state anymore unless the user sets it back to this state with the keyboard input.

    bool whileloop_boolean = true;  // Boolean for the while loop
    sendtomotor(0.0);              // Set motor velocity to 0

    while (whileloop_boolean) {
        if (button1.read() == 0) {          // If button1 is pressed:
            MyState = EMG_CALIBRATION;      // set MyState to EMG_CALIBRATION and exit the while loop
            whileloop_boolean = false;      // by making whileloop_boolean equal to false
        } else if (MyState != MOTORS_OFF) { // If the state is changed by keyboard input, exit the while loop
            whileloop_boolean = false;
        }
    }
}

void measure_data(double &f_y0, double &f_y1, double &f_y2, double &f_y3)
{
    // High pass
    double hb0 = 0.9169;  // Coefficients from the following formula:
    double hb1 = -1.8338; //
    double hb2 = 0.9169;  //        b0 + b1 z^-1 + b2 z^-2
    double ha0 = 1.0;     // H(z) = ----------------------
    double ha1 = -1.8268; //        a0 + a1 z^-1 + a2 z^-2
    double ha2 = 0.8407;  //

    // Low pass
    double lb0 = 0.000083621;  // Coefficients from the following formula:
    double lb1 = 0.0006724; //
    double lb2 = 0.000083621;  //        b0 + b1 z^-1 + b2 z^-2
    double la0 = 1.0;     // H(z) = ----------------------
    double la1 = -1.9740; //        a0 + a1 z^-1 + a2 z^-2
    double la2 = 0.9743;  //

    static double max_y0 = 1;
    static double max_y1 = 1;
    static double max_y2 = 1;
    static double max_y3 = 1;

    static BiQuad hFilter0(hb0,hb1,hb2,ha0,ha1,ha2);  // Create 4 equal filters used for the different EMG signals
    static BiQuad hFilter1(hb0,hb1,hb2,ha0,ha1,ha2);
    static BiQuad hFilter2(hb0,hb1,hb2,ha0,ha1,ha2);
    static BiQuad hFilter3(hb0,hb1,hb2,ha0,ha1,ha2);

    static BiQuad lFilter0(lb0,lb1,lb2,la0,la1,la2);  // Create 4 equal filters used for the different EMG signals
    static BiQuad lFilter1(lb0,lb1,lb2,la0,la1,la2);
    static BiQuad lFilter2(lb0,lb1,lb2,la0,la1,la2);
    static BiQuad lFilter3(lb0,lb1,lb2,la0,la1,la2);

    f_y0 = hFilter0.step(S0);     // Apply filters on the different EMG signals
    f_y1 = hFilter1.step(S1);
    f_y2 = hFilter2.step(S2);
    f_y3 = hFilter3.step(S3);

    f_y0 = abs(f_y0);
    f_y1 = abs(f_y1);
    f_y2 = abs(f_y2);
    f_y3 = abs(f_y3);

    f_y0 = lFilter0.step(f_y0);
    f_y1 = lFilter1.step(f_y1);
    f_y2 = lFilter2.step(f_y2);
    f_y3 = lFilter3.step(f_y3);


    if (MyState == EMG_CALIBRATION) {

        det_max(f_y0, max_y0);       // Determine the maximum RMS value of the EMG signals during the EMG_CALIBRATION state
        det_max(f_y1, max_y1);
        det_max(f_y2, max_y2);
        det_max(f_y3, max_y3);

    } else if ((int)MyState > 4) {
        f_y0 = f_y0/max_y0;     // Normalise the RMS value by dividing by the maximum RMS value
        f_y1 = f_y1/max_y1;     // This is done during the states with a value higher than 4, as this is when you start the operating mode
        f_y2 = f_y2/max_y2;
        f_y3 = f_y3/max_y3;
    }

}

void det_max(double y, double &max_y)
{
    max_y = max_y < y ? y : max_y; // if max_rms is smaller than rms, set rms as new max_rms, otherwise keep max_rms
}

void emgcalibration()
{
    double y0, y1, y2, y3;           // RMS values of the different EMG signals

    measure_data(y0, y1, y2, y3);   // Calculate RMS values

    double duration = 20.0;                 // Duration of the emgcalibration function, in this case 10 seconds
    int rounds = (duration / timeinterval); // Determine the amount of times this function has to run to run for the duration time
    // rounds is an integer so the value of duration / timeinterval is floored

    static int counter = 0;             // Counter which keeps track of the amount of times the function has executed
    if (counter >= rounds) {
        MyState = MOTOR_CALIBRATION;    // If counter is larger than rounds, change MyState to MOTOR_CALIBRATION
    } else {
        counter++;  // Else increase counter by 1
    }

}

//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 get_input(char c)
{
    if (c == 'd') {
        MyState = DEMO_MODE;
    } else if (c == 'o') {
        MyState = OPERATING_MODE;
    } else {
        pc.printf("'d' or 'o' were not pressed\r\nPress 'd' to start the demo mode and press 'o' to start the operating mode\r\n");
        get_input(pc.getc());
    }
}
*/
void startgame()
{
    pc.printf("Please choose which game you would like to start:\r\n");

    pc.printf("- Press 'd' to start the demo mode\r\n     Demo mode is a mode in which the different movements of the robot are shown.\r\n");
    pc.printf("     It will show the edges of the space that the end effector is allowed to go to and will also move in straight lines to show that the robot meets the requirements.\r\n");
    pc.printf("     It will also show how it is able to grab and lift objects which are on the board.\r\n\r\n");

    pc.printf("- Press 'o' to start the operating mode\r\n     The operating mode is a mode in which you can control the robot by flexing the muscles to which the electrodes are attached.\r\n");
    pc.printf("     You will be able to move the arm and use the gripper to try and grab and lift objects from the board to the container.\r\n\r\n");

    while (MyState == START_GAME) {
        char c = pc.getc();
        if (c == 'd') {
            MyState = DEMO_MODE;
        } else if (c == 'o') {
            MyState = OPERATING_MODE;
        }
        else {
            pc.printf("'d' or 'o' were not pressed\r\nPress 'd' to start the demo mode and press 'o' to start the operating mode\r\n");
        }
    }
}

/*
void demo_mode() {

}
*/

void operating_mode()
{
    double y0,y1,y2,y3;
    measure_data(y0,y1,y2,y3);
    double F_Y[4] = {y0, y1, y2, y3};

    double threshold = 0.3;
    for (int i = 0; i < 4; i++) {
        if (F_Y[i] > threshold) {
            //The direction and velocity of the motors are determind here
        }
    }

}

void New_State()
{
    switch (MyState) {
        case MOTORS_OFF :
            pc.printf("\r\nState: Motors turned off\r\n");
            motorsoff();
            break;

        case EMG_CALIBRATION :
            pc.printf("\r\nState: EMG Calibration\r\n");
            pc.printf("Emg calibration mode will run for 20 seconds. Try to flex the muscles to which the electrodes are attached as hard as possible during this time.\r\n");
            measurecontrol.attach(emgcalibration,timeinterval);
            break;

        case MOTOR_CALIBRATION :
            pc.printf("\r\nState: Motor Calibration\r\n");
            break;

        case START_GAME :
            pc.printf("\r\nState: Start game\r\n");
            startgame();
            break;

        case DEMO_MODE :
            pc.printf("\r\nState: Demo mode\r\n");
            //demo_mode();
            break;

        case OPERATING_MODE :
            pc.printf("\r\nState: Operating mode\r\n");
            measurecontrol.attach(operating_mode,timeinterval);
            break;

        case MOVE_GRIPPER :
            pc.printf("\r\nState: Move the gripper\r\n");
            break;

        case END_GAME :
            pc.printf("\r\nState: End of the game\r\n");
            break;

        case FAILURE_MODE :
            pc.printf("\r\nFAILURE MODE!!\r\n");        // 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("\r\nDefault state: Motors are turned off\r\n");
            measurecontrol.attach(nothing,10000);
            sendtomotor(0.0);
            break;
    }
}

void Set_State()
{
    if (MyState == FAILURE_MODE) {
        pc.printf("\r\nNot possible to set state because robot is in FAILURE_MODE. Please restart robot.\r\n");
    } else {
        sendtomotor(0.0);                       // Stop the motors
        measurecontrol.attach(nothing,10000);   // Stop the ticker function from running

        pc.printf("\r\nPress number:     | To go to state:");
        pc.printf("\r\n              (0) | MOTORS_OFF: Set motorspeed just in case to 0 and wait till button1 is pressed");
        pc.printf("\r\n              (1) | EMG_CALIBRATION: Calibrate the maximum of the emg signals and normalise the emg signals with these maxima");
        pc.printf("\r\n              (2) | MOTOR_CALIBRATION: Calibrate the motors to determine the (0,0) position.");
        pc.printf("\r\n              (3) | START_GAME: Determine by keyboard input if you want to go to the demo or operating mode");
        pc.printf("\r\n              (4) | DEMO_MODE: The demo mode will show the different motions that the robot can make");
        pc.printf("\r\n              (5) | OPERATING_MODE: Move the arms and gripper of the arm by flexing your muscles");
        pc.printf("\r\n              (6) | MOVE_GRIPPER: Control the movement of the gripper");
        pc.printf("\r\n              (7) | END_GAME: End effector returns to (0,0) and the motors are turned off, returns to START_GAME mode afterwards");
        pc.printf("\r\n              (8) | FAILURE_MODE: Everything is stopped and the whole programm stops running.\r\n");

        wait(0.5);

        char a = '0';
        char b = '8';
        bool boolean = true;

        while (boolean) {
            char c = pc.getc();

            if (c >= a && c <= b) {
                MyState = (States)(c-'0');
                boolean = false;

            } else {
                pc.printf("\r\nPlease enter a number between 0 and 8\r\n");
            }
        }
    }
}

void failuremode()
{
    MyState = FAILURE_MODE; // failurmode() is activated so set MyState to FAILURE_MODE
    New_State();            // Execute actions coupled to FAILURE_MODE
}