File content as of revision 15:5f9450964075:

// MOTOR_CONTROL FUNCTION HAS TO BE ADJUSTED TO TWO MOTORS
// reference velocity has to be fixed? idk? --> wait for file from bram en paul

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

#include <math.h>
#include <deque>

#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

AnalogIn shield0(A0);   // Input EMG Shield 0
AnalogIn shield1(A1);   // Input EMG Shield 1
AnalogIn shield2(A2);   // Input EMG Shield 2
AnalogIn shield3(A3);   // Input EMG Shield 3

Ticker measurecontrol;              // Ticker function for motor in- and output
DigitalOut motor1Direction(D7);     // Direction of motor 1
FastPWM motor1Velocity(D6);         // FastPWM class to set motor velocity of motor 1
MODSERIAL pc(USBTX, USBRX);
QEI Encoder(D8,D9,NC,8400);         // Input from the encoder to measure how much the motor has turned

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, 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;

// Initialise the functions

void motorsoff();

float rms_deque(std::deque<float> deque);

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

void det_max(float rms, float &max_rms);

void emgcalibration();

double P_controller(double error);

void nothing(){
    // Do nothing
}

void New_State();

void failuremode();




int main()
{
    pc.printf("Starting...\r\n\r\n");
    double frequency = 17000.0f;            // Set motorfrequency
    double period_signal = 1.0f/frequency;  // Convert to period of the signal
    pc.baud(115200);
    
    button2.fall(failuremode);              // Function is always activated when the button is pressed
    motor1Velocity.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.0f);                          // 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.0f);              // 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
        }
    }
}

float rms_deque(std::deque<float> deque) {
    float sum = 0;
    for (int i = 0; i < deque.size(); i++) {
        sum = sum + pow(deque[i],2);        // Square the entries of the deque and add them to sum
    }
    return pow(sum,0.5f);                   // Return the square root of the sum (and thus the RMS)
}

void measure_data(float &rms_0, float &rms_1, float &rms_2, float &rms_3) {
    float b0 = 0.0f;  // Coefficients from the following formula:
    float b1 = 0.0f;  //
    float b2 = 0.0f;  //        b0 + b1 z^-1 + b2 z^-2
    float a0 = 0.0f;  // H(z) = ----------------------
    float a1 = 0.0f;  //        a0 + a1 z^-1 + a2 z^-2
    
    static float max_rms0 = 0;
    static float max_rms1 = 0;
    static float max_rms2 = 0;
    static float max_rms3 = 0;
    
    static BiQuad Filter0(b0,b1,b2,a0,a1);  // Create 4 equal filters used for the different EMG signals
    static BiQuad Filter1(b0,b1,b2,a0,a1);
    static BiQuad Filter2(b0,b1,b2,a0,a1);
    static BiQuad Filter3(b0,b1,b2,a0,a1);
    
    float f_y0 = Filter0.step(shield0);     // Apply filters on the different EMG signals
    float f_y1 = Filter1.step(shield1);
    float f_y2 = Filter2.step(shield2);
    float f_y3 = Filter3.step(shield3);
    
    int rms_length = 6;                     // Set the amount of points of which the RMS signal is calculated
    static std::deque<float> deque_f_y0 (rms_length);   // Create deque for the 4 signals in which data can be added and removed
    static std::deque<float> deque_f_y1 (rms_length);
    static std::deque<float> deque_f_y2 (rms_length);
    static std::deque<float> deque_f_y3 (rms_length);
    
    deque_f_y0.push_front(f_y0);    // Take new data point and put it in front of the deque values
    deque_f_y1.push_front(f_y1);
    deque_f_y2.push_front(f_y2);
    deque_f_y3.push_front(f_y3);
    
    deque_f_y0.pop_back();          // Remove latest element in deque to keep the deque the same length
    deque_f_y1.pop_back();
    deque_f_y2.pop_back();
    deque_f_y3.pop_back();
    
    rms_0 = rms_deque(deque_f_y0);    // Calculate the RMS for the different deques
    rms_1 = rms_deque(deque_f_y1);    // and give this value to rms_1 which is a reference
    rms_2 = rms_deque(deque_f_y2);    // 
    rms_3 = rms_deque(deque_f_y3);
    
    if (MyState == EMG_CALIBRATION) {
        
        det_max(rms_0, max_rms0);       // Determine the maximum RMS value of the EMG signals during the EMG_CALIBRATION state
        det_max(rms_1, max_rms1);
        det_max(rms_2, max_rms2);
        det_max(rms_3, max_rms3);
        
    }
    else if ((int)MyState > 4) {
        rms_0 = rms_0/max_rms0;     // Normalise the RMS value by dividing by the maximum RMS value
        rms_1 = rms_1/max_rms1;     // This is done during the states with a value higher than 4, as this is when you start the playable game mode
        rms_2 = rms_2/max_rms2;
        rms_3 = rms_3/max_rms3;
    }
    
}

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

void emgcalibration() {
    float rms0, rms1, rms2, rms3;           // RMS values of the different EMG signals

    measure_data(rms0, rms1, rms2, rms3);   // Calculate RMS values
    
    float duration = 10.0f;                 // 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 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");
            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.0f);
            break;
    }
}

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