File content as of revision 25:45c131af2dba:

// Operating mode might not go to next state when SW2 is pressed

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

#include <cmath>    // Included to use different math operations

#include "Servo.h"  // Included to control the servo motor 

Servo myservo(D13);     // To control the servo motor
DigitalIn but3(SW2);    // To go to the next state or to choose one of two game modes when in state START_GAME
DigitalIn but4(SW3);    // To choose one of two game modes when in state START_GAME or to move the gripper

AnalogIn S0(A0);
AnalogIn S1(A1);
AnalogIn S2(A2);
AnalogIn S3(A3);

DigitalOut motor1Direction(D7);
FastPWM motor1Velocity(D6);
DigitalOut motor2Direction(D4);
FastPWM motor2Velocity(D5);

// Encoders 1 and 2 respectively
QEI Encoder1(D8,D9,NC,8400);
QEI Encoder2(D11,D10,NC,8400);

Ticker measurecontrol;              // Ticker function for the measurements

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

Serial pc(USBTX, USBRX);

double PI = 3.14159265358;//97932384626433832795028841971693993;
volatile double timeinterval = 0.001f;      // Time interval of the Ticker function
volatile double frequency = 17000.0;        // Set motorfrequency
double period_signal = 1.0/frequency;       // Convert to period of the signal

double yendeffector = 10.6159;
double xendeffector = 0;
int ingedrukt = 0;
int state_int;
int previous_state_int;
double motorvalue1;
double motorvalue2;
// Define the different states in which the robot can be
enum States {MOTORS_OFF, EMG_CALIBRATION, START_GAME,
             DEMO_MODE, OPERATING_MODE, END_GAME
            };

// 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 &f_y0, double &f_y1, double &f_y2, double &f_y3);

void det_max(double y, double &max_y);

void emgcalibration();

void nothing()
{
    // Do nothing
}

void startgame() ;

void demo_mode();

void operating_mode();

void New_State();

void Set_State();

double PID_controller1(double error1);

double PID_controller2(double error2);

void getmeasuredposition(double & measuredposition1, double & measuredposition2);

void getreferenceposition(double & referenceposition1, double & referenceposition2);

void sendtomotor(double motorvalue1, double motorvalue2);

void measureandcontrol();



int main()
{
    pc.baud(115200);
    pc.printf("Starting...\r\n\r\n");

        motor1Velocity.period(period_signal);   // Set the period of the PWMfunction
        motor2Velocity.period(period_signal);   // Set the period of the PWMfunction

    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(true) {
        if ( (previous_state_int - (int)MyState) != 0 ) {   // If current state is not previous state execute New_State()
            New_State();
        }
    }
}



double PID_controller1(double error1)
{
    // Define errors for motor 1 and 2
    static double error_integral1 = 0;
    static double error_prev1 = error1;

    // Low-pass filter
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

    // PID variables: we assume them to be the same for both motors
    double Kp = 63;
    double Ki = 3.64;
    double Kd = 5;

    //Proportional part:
    double u_k1 = Kp * error1;

    // Integreal part
    error_integral1 = error_integral1 + error1 * timeinterval;
    double u_i1 = Ki*error_integral1;

    // Derivate part
    double error_derivative1 = (error1 - error_prev1)/timeinterval;
    double filtered_error_derivative1 = LowPassFilter.step(error_derivative1);
    double u_d1 = Kd * filtered_error_derivative1;
    error_prev1 = error1;

    //sum all parts and return it
    return u_k1 + u_i1 + u_d1;
}


double PID_controller2(double error2)
{
    // Define errors for motor 1 and 2
    static double error_integral2 = 0;
    static double error_prev2 = error2;

    // Low-pass filter
    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

    // PID variables: we assume them to be the same for both motors
    double Kp = 63;
    double Ki = 3.64;
    double Kd = 5;

    //Proportional part:
    double u_k2 = Kp * error2;

    // Integreal part
    error_integral2 = error_integral2 + error2 * timeinterval;
    double u_i2 = Ki*error_integral2;

    // Derivate part
    double error_derivative2 = (error2 - error_prev2)/timeinterval;
    double filtered_error_derivative2 = LowPassFilter.step(error_derivative2);
    double u_d2 = Kd * filtered_error_derivative2;
    error_prev2 = error2;

    //sum all parts and return it
    return u_k2 + u_i2 + u_d2;
}


//get the measured position
void getmeasuredposition(double & measuredposition1, double & measuredposition2)
{
    // Obtain the counts of motors 1 and 2 from the encoder
    int countsmotor1;
    int countsmotor2;
    countsmotor1 = Encoder1.getPulses();
    countsmotor2 = Encoder2.getPulses();

    // Obtain the measured position for motor 1 and 2
    measuredposition1 = ((double)countsmotor1) / 8400.0f * 2.0f;
    measuredposition2 = ((double)countsmotor2) / 8400.0f * 2.0f;
}


//get the reference of the
void getreferenceposition(double & referenceposition1, double & referenceposition2)
{
    //Measurements of the arm
    double L0=1.95;
    double L1=15;
    double L2=20;

    //Inverse kinematics: given the end position, what are the desired motor angles of 1 and 2
    double desiredmotorangle1, desiredmotorangle2;
    desiredmotorangle1 = (atan2(yendeffector,(L0+xendeffector))*180.0/PI + acos((pow(L1,2)+pow(L0+xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0+xendeffector,2)+pow(yendeffector,2))))*180.0/PI)-180.0;
    desiredmotorangle2 = (atan2(yendeffector,(L0-xendeffector))*180.0/PI + acos((pow(L1,2)+pow(L0-xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0-xendeffector,2)+pow(yendeffector,2))))*180.0/PI)-180.0;

    //Convert motor angles to counts
    double desiredmotorrounds1;
    double desiredmotorrounds2;
    desiredmotorrounds1 = (desiredmotorangle1)/360.0;
    desiredmotorrounds2 = (desiredmotorangle2)/360.0;

    //Assign this to new variables because otherwise it doesn't work
    referenceposition1 = desiredmotorrounds1;
    referenceposition2 = desiredmotorrounds2;
}


//send value to motor
// IT WAS "void sendtomotor(float & motorvalue1, float & motorvalue2)" BUT I REMOVED THE REFERENCE, BECAUSE I THOUGHT IT WAS NOT NEEDED
void sendtomotor(double motorvalue1, double motorvalue2)
{
    // Define the absolute motor values
    double absolutemotorvalue1;
    double absolutemotorvalue2;
    absolutemotorvalue1 = fabs(motorvalue1);
    absolutemotorvalue2 = fabs(motorvalue2);

    // If absolutemotorvalueocity is greater than 1, reduce to 1, otherwise remain absolutemotorvalue
    absolutemotorvalue1 = absolutemotorvalue1 > 1.0f ? 1.0f : absolutemotorvalue1;
    absolutemotorvalue2 = absolutemotorvalue2 > 1.0f ? 1.0f : absolutemotorvalue2;

    // Send the absolutemotorvalue to the motors
    motor1Velocity = absolutemotorvalue1;
    motor2Velocity = absolutemotorvalue2;

    // Determine the motor direction. Boolean output: true gives counterclockwise direction, false gives clockwise direction
    motor1Direction = (motorvalue1 > 0.0f);
    motor2Direction = (motorvalue2 > 0.0f);
}

// function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
void measureandcontrol()
{
    // Get the reference positions of motor 1 and 2
    double reference1, reference2;
    getreferenceposition(reference1, reference2);

    // Get the measured positions of motor 1 and 2
    double measured1, measured2;
    getmeasuredposition(measured1, measured2);

    // Calculate the motor values
    //double motorvalue1, motorvalue2;
    motorvalue1 = PID_controller1(reference1 - measured1);
    motorvalue2 = PID_controller2(reference2 - measured2);
    sendtomotor(motorvalue1, motorvalue2);
}


void motorsoff()
{
    // Function to turn the motors off. First state that the robot has. Robot will stay in this state untill button SW2 is pressed.
    // Robot will not return to this state anymore unless the user sets it back to this state with the keyboard input.
    sendtomotor(0.0,0.0);           // Set motor velocity to 0

    state_int = 10;

    bool whileloop_boolean = true;  // Boolean for the while loop

    while (whileloop_boolean) {
        if (but3.read() == 0) {                     // If button SW2 is pressed:
            MyState = (States)((int)MyState+1);     // set MyState to EMG_CALIBRATION and exit the while loop
            whileloop_boolean = false;              // by making whileloop_boolean equal to false
            wait(0.5f);
        }
    }
}

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 = 0.001;
    static double max_y1 = 0.001;
    static double max_y2 = 0.001;
    static double max_y3 = 0.001;

    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 > 3) {
        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 3, 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 = 15.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 = START_GAME;    // If counter is larger than rounds, change MyState to the next state
        measurecontrol.detach();
    } else {
        counter++;  // Else increase counter by 1
    }

}

void startgame()
{
    pc.printf("Please choose which game you would like to start:\r\n");

    pc.printf("- Press button SW2 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 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 button SW3 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) {

        if (but3.read() == 0) {
            MyState = (States)((int)MyState+1);
            wait(0.5f);
        } else if (but4.read() == 0) {
            MyState = (States)((int)MyState+2);
            wait(0.5f);
        }

    }
}


void demo_mode()
{

    //state_int = 10;

    // 5 pre determined positions of the end effector to show it can move in straight lines
    xendeffector=-5;
    yendeffector=10.6159;
    wait(0.5);

    xendeffector=10;
    yendeffector=25.6159;
    wait(0.5);

    xendeffector=-14;
    yendeffector=21.6159;
    wait(0.5);

    xendeffector=10;
    yendeffector=11.6159;
    wait(0.5);

    // Last position is the start position
    xendeffector=0;
    yendeffector=10.6159;
    wait(0.5);

    MyState = START_GAME;     // Go back to START_GAME mode
}


void operating_mode()
{
    double y0,y1,y2,y3;
    measure_data(y0,y1,y2,y3);
    
    
    double threshold = 0.3;     // When the analysed signal is above this threshold, the position of the end effector is changed
    double dr = 0.01;            // The change in position with each step
    
    if(y0 > threshold && xendeffector < 16) {
        xendeffector=xendeffector+dr;
    }
    else if(y1 > threshold && xendeffector > -16) {
        xendeffector=xendeffector-dr;
    }
    if(y2 > threshold && yendeffector < 28) {
        yendeffector=yendeffector+dr;
    }
    else if(y3 > threshold && yendeffector > 8) {
        yendeffector=yendeffector-dr;
    }

    //servo besturing
    
    
    
    if(but4.read() == 0 && ingedrukt == 0) {
        for(int i=0; i<100; i++) {
            myservo = i/100.0;
        }
        ingedrukt = 1;
    } else if(but4.read() == 0 && ingedrukt == 1) {
        for(int i=100; i>0; i--) {
            myservo = i/100.0;
        }
        
        ingedrukt = 0;
    }
    
    if (but3.read() == 0) {
        bool buttonss = true;
        while (buttonss) {
            if (but3.read() == 1) {
                buttonss = false;
            }
        }
        pc.printf("The game has ended, will move the end effector to (0,0), put the motors off and will now return to the state START_GAME");
        MyState = END_GAME;
        //xendeffector=0.0;
        //yendeffector=10.6159;
    }
    /*
    if (but3.read() == 0) {
        pc.printf("The game has ended, will move the end effector to (0,10.6159), put the motors off and will now return to the state START_GAME");
        MyState = END_GAME;
        xendeffector=0.0;
        yendeffector=10.6159;
    }
    */
    measureandcontrol();
}

void endgame()
{
    /*
    current_y = yendeffector;
    current_x = xendeffector;
    
    while (abs(current_y-10.6159) >= 0.01 && abs(current_x-10.6159) >= 0.01) [
        
    }
    */
    xendeffector = 0.0;
    yendeffector = 20.6159;
    //motorvalue1 = 0;
    //motorvalue2 = 0;
    wait(0.5);
    measurecontrol.detach();    
    MyState = START_GAME;
}

void New_State()
{
    previous_state_int = (int)MyState;                  // Change previous state to current 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 START_GAME :
            pc.printf("\r\nState: Start game\r\n");
            startgame();
            break;

        case DEMO_MODE :
            pc.printf("\r\nState: Demo mode\r\n");
            measurecontrol.attach(measureandcontrol,timeinterval);
            demo_mode();
            measurecontrol.detach();
            break;

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

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

        default :
            pc.printf("\r\nDefault state: Motors are turned off\r\n");
             
            sendtomotor(0.0,0.0);
            break;
    }
}

void Set_State()
{
    xendeffector=0.0;
    yendeffector=10.6159;
    wait(0.3f);
    sendtomotor(0.0,0.0);                       // Stop the motors
        // 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 button SW2 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) | START_GAME: Determine by keyboard input if you want to go to the demo or operating mode");
    pc.printf("\r\n              (3) | DEMO_MODE: The demo mode will show the different motions that the robot can make");
    pc.printf("\r\n              (4) | OPERATING_MODE: Move the arms and gripper of the arm by flexing your muscles");
    pc.printf("\r\n              (5) | END_GAME: End effector returns to (0,0) and the motors are turned off, returns to START_GAME mode afterwards");

    wait(0.5f);

    char a = '0';
    char b = '5';
    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 5\r\n");
        }
    }
}