File content as of revision 26:91d48c0b722d:

#include "mbed.h"
#include "HIDScope.h"
#include "BiQuad.h"
#include "MODSERIAL.h"
#include <string.h>

//Define objects

InterruptIn button_calibrate(PTA4);
AnalogIn    emg1( A0 );
AnalogIn    emg2( A1 );
AnalogIn    emg3( A2 );

Ticker      sample_timer;
HIDScope    scope( 6 );
MODSERIAL pc(USBTX, USBRX);
DigitalOut  led(LED1);

volatile bool sampletimer = false;
volatile bool buttonflag = false;
volatile bool newcase = false;

double threshold = 0.04;
double samplefreq=0.002;
double emg02;
double emg12;
double emg22;
//double ref_x=0.000;
//double ref_y=0.000;
typedef enum { STATE_CALIBRATION, STATE_PAUZE, STATE_X, STATE_X_NEG, STATE_Y, STATE_Y_NEG, STATE_XY, STATE_XY_NEG } states;

BiQuadChain bqc11;
BiQuadChain bqc13;
BiQuadChain bqc21;
BiQuadChain bqc23;
BiQuadChain bqc31;
BiQuadChain bqc33;
//BiQuad bq11( 9.87589e-01, -1.59795e+00, 9.87589e-01, -1.59795e+00, 9.75178e-01 ); //oude BiQuad waardes
/*  BiQuads for filter emg1
    notch filter*/
BiQuad bq111(0.9795,   -1.5849,    0.9795,    1.0000,   -1.5849,    0.9589);
BiQuad bq112(0.9833,   -1.5912,    0.9833,    1.0000,   -1.5793,    0.9787);
BiQuad bq113(0.9957,   -1.6111,    0.9957,    1.0000,   -1.6224,    0.9798);
/*  High pass filter*/
BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 );
/*  low pass filter*/
BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );


/*  BiQuads for filter emg2
    notch filter*/
BiQuad bq211(0.9795,   -1.5849,    0.9795,    1.0000,   -1.5849,    0.9589);
BiQuad bq212(0.9833,   -1.5912,    0.9833,    1.0000,   -1.5793,    0.9787);
BiQuad bq213(0.9957,   -1.6111,    0.9957,    1.0000,   -1.6224,    0.9798);
/*  High pass filter*/
BiQuad bq221( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 );
/*  low pass filter*/
BiQuad bq231( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );

/*  BiQuads for filter emg3
    notch filter*/
BiQuad bq311(0.9795,   -1.5849,    0.9795,    1.0000,   -1.5849,    0.9589);
BiQuad bq312(0.9833,   -1.5912,    0.9833,    1.0000,   -1.5793,    0.9787);
BiQuad bq313(0.9957,   -1.6111,    0.9957,    1.0000,   -1.6224,    0.9798);
/*  High pass filter*/
BiQuad bq321( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 );
/*  low pass filter*/
BiQuad bq331( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );


void sampleflag()
{
    sampletimer=true;
}

void buttonflag_go()
{
    buttonflag=true;
}

void sample_button(states &mystate)
{
    states myoldstate=mystate;
    char key=pc.getc();

    // pc.printf("%c/n",key);

    switch (key) {
        case 'p' : // run
            emg02=0.0;
            emg12=0.0;
            emg22=0.0;
            break;
        case 'q' : // run
            emg02=0.0;
            emg12=1.0;
            emg22=0.0;
            break;
        case 'w' : // run
            emg02=0.0;
            emg12=0.0;
            emg22=1.0;
            break;
        case 'e' : // run
            emg02=0.0;
            emg12=1.0;
            emg22=1.0;
            break;
        case 'a' : // run
            emg02=1.0;
            emg12=1.0;
            emg22=0.0;
            break;
        case 's' : // run
            emg02=1.0;
            emg12=0.0;
            emg22=1.0;
            break;
        case 'd' : // run
            emg02=1.0;
            emg12=1.0;
            emg22=1.0;
            break;
    }





    led = !led;
    if (emg02>threshold) {
        if (emg12>threshold&&emg22>threshold) {
            mystate = STATE_XY_NEG;
        } else if (emg12>threshold) {
            mystate = STATE_X_NEG;

        } else if (emg22>threshold) {
            mystate = STATE_Y_NEG;
        } else {
            mystate = STATE_PAUZE;
        }
    } else {
        if (emg12>threshold&&emg22>threshold) {
            mystate = STATE_XY;
        } else if (emg12>threshold) {
            mystate = STATE_X;

        } else if (emg22>threshold) {
            mystate = STATE_Y;
        } else {
            mystate = STATE_PAUZE;
        }
    }
    
    if (buttonflag==true) {
        mystate = STATE_CALIBRATION;
        newcase=true;
    }

    if (myoldstate==mystate) {
        newcase=false;
    } else {
        newcase=true;
    }
    sampletimer = false;
}

void sample(states &mystate)
{

    /* Read the emg signals and filter it*/

    scope.set(0, emg1.read());    //original signal
    double emg02=bqc13.step(fabs(bqc11.step(emg1.read())));
    scope.set(1, emg02);
    /* Read the second emg signal and filter it*/
    scope.set(2, emg2.read());    //original signal
    double emg12=bqc23.step(fabs(bqc21.step(emg2.read())));
    scope.set(3, emg12);
    /* Read the third emg signal and filter it*/
    scope.set(4, emg3.read());    //original signal
    double emg22=bqc33.step(fabs(bqc31.step(emg3.read())));
    scope.set(5, emg22);
    //pc.printf("Hello World!\n");
    /*   Ensure that enough channels are available (HIDScope scope( 2 ))
     *   Finally, send all channels to the PC at once */
    scope.send();
    /*   To indicate that the function is working, the LED is toggled */

    led = !led;
    if (emg02>threshold) {
        if (emg12>threshold&&emg22>threshold) {
            mystate = STATE_XY_NEG;
        } else if (emg12>threshold) {
            mystate = STATE_X_NEG;

        } else if (emg22>threshold) {
            mystate = STATE_Y_NEG;
        } else {
            mystate = STATE_PAUZE;
        }
    } else {
        if (emg12>threshold&&emg22>threshold) {
            mystate = STATE_XY;
        } else if (emg12>threshold) {
            mystate = STATE_X;

        } else if (emg22>threshold) {
            mystate = STATE_Y;
        } else {
            mystate = STATE_PAUZE;
        }
    }

    if (buttonflag==true) {
        mystate = STATE_CALIBRATION;
    }

    sampletimer = false;
}


int main()
{
    pc.baud(115200);

    // create a variable called 'state', define it
    states mystate = STATE_PAUZE;

    //de biquad chains instellen
    bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 );
    bqc13.add( &bq131);
    bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 );
    bqc23.add( &bq231);
    bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 );
    bqc33.add( &bq331);
    /*Attach the 'sample' function to the timer 'sample_timer'.
      this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
    */
    sample_timer.attach(&sampleflag, samplefreq);
    button_calibrate.fall(&buttonflag_go);


    while(1) {
        if (sampletimer==true) {
            //sample(mystate);
            sample_button(mystate);
        }
        // switch, case
        switch (mystate) {
            case STATE_CALIBRATION : { // calibration
                pc.printf("calibration\n");
                while (button_calibrate==0) {}
                newcase=false;
                buttonflag=false;
                break;
            }
            case STATE_X : // run
                if (newcase==true) {
                    pc.printf("X\n");
                    newcase=false;
                }

                break;
            case STATE_X_NEG : // run
                if (newcase==true) {
                    pc.printf("Xneg\n");
                    newcase=false;
                }
                break;
            case STATE_Y : // execute mode 1
                if (newcase==true) {
                    pc.printf("Y\n");
                    newcase=false;
                }
                break;
            case STATE_Y_NEG : // execute mode 1
                if (newcase==true) {
                    pc.printf("Yneg\n");
                    newcase=false;
                }
                break;
            case STATE_XY : // execute mode 2
                if (newcase==true) {
                    pc.printf("XY\n");
                    newcase=false;
                }
                break;
            case STATE_XY_NEG : // execute mode 2
                if (newcase==true) {
                    pc.printf("XYneg\n");
                    newcase=false;
                }
                break;
            case STATE_PAUZE : // default
                if (newcase==true) {
                    pc.printf("PAUZE\n");
                    newcase=false;
                }
                break;
        }

    }
}