File content as of revision 0:f49bda586fc5:

#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 calibrationgo;      // Timeout that will determine the duration of the calibration.

//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_processStateSwitch()
{
    while(go_switch) {
        switch(state) {
            case R_HORIZONTAL:
                state = R_VERTICAL;
                ledblue = 1;
                ledred = 1;
                ledgreen = 0;
                pc.printf("state is vertical");
                break;
            case R_VERTICAL:
                state = R_BASE;
                ledgreen = 1;
                ledblue = 1;
                ledred = 0;
                pc.printf("state is base");
                break;
            case R_BASE:
                state = R_HORIZONTAL;
                ledgreen = 1;
                ledred = 1;
                ledblue = 0;
                pc.printf("state is horizontal");
                break;
                }
                wait(3.0f);
                go_switch = false;
                ledstatedef = 1;
                ledstateswitch = 0;
        }   
}

void processEMG ()
{
    r_processStateSwitch();
    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(115200);                    // Set baudrate for proper communication
    go_EMG = true;                      // Setting ticker variables
    go_calibration = true;              // Setting the timeout variable
    calibrationgo.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;

        }


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


    }
}