File content as of revision 1:90041f90ecf2:

#include "mbed.h"
#include "math.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "EMG_filter.h"

MODSERIAL pc(USBTX,USBRX);
//HIDScope scope (2);

enum r_states {R_HORIZONTAL, R_VERTICAL, R_BASE};
r_states state;


//Define EMG inputs
EMG_filter emg1(A0);
EMG_filter emg2(A1);
DigitalOut ledred(LED_RED);
DigitalOut ledgreen(LED_GREEN);
DigitalOut ledblue(LED_BLUE);
DigitalOut ledstateswitch(D8);
DigitalOut ledstatedef(D11);


//EMG_filter emg3(A2);

//Define ticker1
Ticker mainticker;      //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_EMG
Timeout maintimeout;      // Timeout that will determine the duration of the calibration.
Timeout testtimeout;

//Define ticker variables
bool go_EMG;                    // Boolean to put on/off the EMG readout
bool go_calibration;            // Boolean to put on/off the calibration of the EMG
bool go_switch;

//Function that reads out the raw EMG and filters the signal


void calibrationGO()                   // Function for go or no go of calibration
{
    go_calibration = false;
}

/*
Calibration of the robot works as follows:
EMG signal is read out for 5 seconds, 1000 times a second. This signal is filtered and the maximum value is compared to the current value of the MVC.
The value of the MVC is overwritten when the value of the filtered signal is bigger than the current value.
During the calibration the user should exert maximum force.
*/

void calibrationEMG()                   // Function for calibrating the EMG signal
{
    if (go_calibration) {
        emg1.calibration();                 // Using the calibration function of the EMG_filter class
    }
}


void r_movehorizontal()
{


}

void r_movevertical()
{

}

void r_movebase()
{

}
/*
void r_processStateSwitchGO()   // function will be runned after 3 seconds.
{
    
    go_switch = false;          // then it will stop again
    ledstatedef = 1;            // now you are definitely in a state and motor controll will begin again.
    ledstateswitch = 0;   
}
*/
void r_processStateSwitch()
{
    
    if(go_switch == true) {
        switch(state) {
            case R_HORIZONTAL:
                state = R_VERTICAL;
                ledblue = 1;
                ledred = 1;
                ledgreen = 0;
                //pc.printf("state is vertical\r\n");
                break;
            case R_VERTICAL:
                state = R_BASE;
                ledgreen = 1;
                ledblue = 1;
                ledred = 0;
                //pc.printf("state is base\r\n");
                break;
            case R_BASE:
                state = R_HORIZONTAL;
                ledgreen = 1;
                ledred = 1;
                ledblue = 0;
                //pc.printf("state is horizontal\r\n");
                break;
        }
            go_switch = false;                      // otherwise it will see a lot of values over 0.7 so it will switch states 100 times per peak. 
            ledstatedef = 1;            // now you are definitely in a state and motor controll will begin again.
            ledstateswitch = 0; 
    }
}

void processEMG ()
{

    if (go_EMG) {
     
        //go_EMG = false;                   //set the variable to false --> misschien nodig?
        emg1.filter();                      //filter the incoming emg signal
        emg2.filter();
        //emg3.filter();
        /*
        scope.set(0, emg1.normalized);      //views emg1.normalized on port 0 of HIDScope
        scope.set(1, emg1.emg0);
        scope.send();*/
    }

}



int main()
{
    pc.baud(9600);                    // Set baudrate for proper communication
    go_EMG = true;                      // Setting ticker variables
    go_calibration = true;              // Setting the timeout variable
    maintimeout.attach(&calibrationGO, 5.0);     // Attach calibration timeout to calibration function
    mainticker.attach(&calibrationEMG, 0.001f);    // Attach ticker to the calibration function
    wait(5.0);                                     // Wait for 5.0 seconds to make time for calibration
    mainticker.attach(&processEMG, 0.001f);        // Attaching mainticker to function processEMG, calling  the function 1000 times a second

    while (true) {
        ledstatedef = 1;
        if(emg1.normalized >= 0.7 && emg2.normalized >= 0.7) {
            ledstateswitch = 1;
            ledstatedef = 0;
            go_switch = true;
            r_processStateSwitch(); 
            wait(2.0f);
        
             //r_processStateSwitchGO();           
        }
        pc.printf("go_Switch = %d, emg1 = %f, emg2 = %f \r\n", go_switch, emg1.normalized, emg2.normalized);

        state = R_VERTICAL;
        switch(state) {
            case R_HORIZONTAL:
                r_movehorizontal();
                break;
            case R_VERTICAL:
                r_movevertical();
                break;
            case R_BASE:
                r_movebase();
                break;
        }


    }
}