File content as of revision 17:3b463660aa42:

// 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 <math.h>
//#include <deque>
//#include <array>


//#include "Motor_Control.h"

DigitalIn button1(D12);     // Button1 input to go to next state
InterruptIn button2(SW2);   // Button2 input to activate failuremode()
DigitalOut ledr(LED_RED);   // Red LED output to show

// CHECK DIGITAL PORTS
AnalogIn S0(A0), S1(A1), S2(A2) ,S3(A3);   // Input EMG Shield 0,1,2,3
DigitalOut  MD0(D1), MD1(D2), MD2(D3); // MotorDirection of motors 0,1,2
FastPWM MV0(D4), MV1(D5), MV2(D6); // MotorVelocities of motors 0,1,2
QEI E0(D7,D8,NC,8400), E1(D9,D10,NC,8400), E3(D11,D13,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[3] = {MD0, MD1, MD2};
FastPWM MotorVelocities[3] = {MV0, MV1, MV2};
QEI Encoders[3] = {E0, E1, E3};

MODSERIAL pc(USBTX, USBRX);

double PI = 3.1415926;//535897932384626433832795028841971693993;
double 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, OPERATING_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;

// Initialise the functions

double getmeasuredvelocity();

double getreferencevelocity();

void sendtomotor(double motorvalue);

void measureandcontrol();

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();

double P_controller(double error);

void nothing(){
    // Do nothing
}

void get_input(char c);

void startgame() ;

void New_State();

void failuremode();


//__________________________________________________________________________________________________________________________________
//__________________________________________________________________________________________________________________________________
int main()
{
    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
    
    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;
}
//__________________________________________________________________________________________________________________________________
//__________________________________________________________________________________________________________________________________


//get the measured velocity
double getmeasuredvelocity()
{   
    //iets
    return 0;
}

//get the reference of the velocity: positive or negative
double getreferencevelocity()
{
    //iets
    return 0;
}   

//send value to motor
void sendtomotor(double motorvalue)
{   
    //iets
}

// function to call reference velocity, measured velocity and controls motor with feedback
void measureandcontrol()
{
    //iets
}




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
        }
    }
}

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;
    static double max_y1 = 0;
    static double max_y2 = 0;
    static double max_y3 = 0;
    
    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 = 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 = 10.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");

    get_input(pc.getc());
}

/*
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.5;
        for (int i = 0; i < 4; i++) {
            if (F_Y[i] > threshold) {
                //hey
            }
        } 
    
}


void New_State() {
    switch (MyState)
    {
        case MOTORS_OFF :
            pc.printf("State: Motors turned off");
            motorsoff();
            break;
        
        case EMG_CALIBRATION :
            pc.printf("State: EMG Calibration");
            measurecontrol.attach(emgcalibration,timeinterval);
            break;
        
        case MOTOR_CALIBRATION :
            pc.printf("State: Motor Calibration");
            break;
        
        case START_GAME :
            pc.printf("State: Start game");
            startgame();
            break;
        
        case DEMO_MODE :
            pc.printf("State: Demo mode");
            //demo_mode();
            break;
        
        case OPERATING_MODE :
            pc.printf("State: Operating mode");
            measurecontrol.attach(operating_mode,timeinterval);
            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.0);
            break;
    }
}

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