File content as of revision 4:24e2a5e50387:

#include "mbed.h"
#include "BiQuad.h"
#include "HIDScope.h"
//#define SERIAL_BAUD 115200  // baud rate for serial communication

//Serial communication with PC
//Serial pc(USBTX,USBRX);
AnalogIn    emg0( A0 );
AnalogIn    emg1( A1 );
HIDScope    scope( 5 );

Ticker      filter_timer, send_timer;
DigitalOut  ledRed(LED_RED);
DigitalOut  ledBlue(LED_BLUE);

volatile bool filter_timer_go=false,send_timer_go=false;

//double EMGright, EMGleft, inR;
int EMGgain=1; 
//set initial conditions
//double biceps_l = 0;
//double biceps_r = 0;
//double outRenvelope, outLenvelope;
int T=4;
double threshold_l=0.09;
double threshold_r = 0.09;



void filter_timer_act(){filter_timer_go=true;};
void send_timer_act(){send_timer_go=true;};

BiQuadChain bcq1R;
BiQuadChain bcq2R;
// Notch filter wo=50; bw=wo/35
BiQuad bq1R(9.9821e-01,-1.9807e+00,9.9821e-01,-1.9807e+00,9.9642e-01);
// High pass Butterworth filter 2nd order, Fc=10;
BiQuad bq2R(9.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01);
// Low pass Butterworth filter 2nd order, Fc = 8;
BiQuad bq3R(5.6248e-05,1.1250e-04,5.6248e-05,-1.9787e+00,9.7890e-01);

BiQuadChain bcq1L;
BiQuadChain bcq2L;
// Notch filter wo=50; bw=wo/35
BiQuad bq1L(9.9821e-01,-1.9807e+00,9.9821e-01,-1.9807e+00,9.9642e-01);
// High pass Butterworth filter 2nd order, Fc=10;
BiQuad bq2L(9.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01);
// Low pass Butterworth filter 2nd order, Fc = 8;
BiQuad bq3L(5.6248e-05,1.1250e-04,5.6248e-05,-1.9787e+00,9.7890e-01);

// In the following: R is used for right arm, L is used for left arm!

void FilteredSample(int &Tout)
{   
    double inLout = emg0.read();
    double inRout = emg1.read();
    
    double outRfilter1 = bcq1R.step(inRout);
    double outRrect= fabs(outRfilter1);
    double envelopeR = bcq2R.step(outRrect);
    
    double outLfilter1 = bcq1L.step(inLout);
    double outLrect = fabs(outLfilter1);
    double envelopeL = bcq2L.step(outLrect);
    
    double biceps_l = (double) envelopeL * EMGgain; //emg0.read();     //velocity or reference position change, EMG with a gain
    double biceps_r = (double) envelopeR * EMGgain; //emg1.read();
    if (biceps_l > threshold_l && biceps_r > threshold_r){
        //both arms activated: stamp moves down
        //pc.printf("Stamp down   ");
        //pc.printf("right: %f    ",biceps_r);
        //pc.printf("left:  %f\n\r",biceps_l);
        //wait(0.5);
        Tout = 2;
        //pc.printf("T=%d\n\r",T);
        ledRed=!ledRed;//blink purple
        ledBlue=!ledBlue;
        }
    else if (biceps_l > threshold_l && biceps_r <= threshold_r){
        //arm 1 activated, move left
        //pc.printf("Move left    ");
        //pc.printf("right: %f    ",biceps_r);
        //pc.printf("left:  %f\n\r",biceps_l);
        //wait(0.5);
        Tout  = -1;
        //pc.printf("T=%d\n\r",T);
        ledBlue=1;//off
        ledRed=0;//on    red
        }
    else if (biceps_l <= threshold_l && biceps_r > threshold_r){
        //arm 1 activated, move right
        //pc.printf("Move right   "); 
        //pc.printf("right: %f    ",biceps_r);
        //pc.printf("left:  %f\n\r",biceps_l);
        //wait(0.5);
        Tout = 1;
        //pc.printf("T=%d\n\r",T);
        ledBlue=0;//on    blue
        ledRed=1;//off
        }
    else{
        //wait(0.2);
        ledRed = 1;
        ledBlue = 1;  //off
        //pc.printf("Nothing...   ");
        //wait(0.5);
        Tout = -2;
        //pc.printf("right: %f    ",biceps_r);
        //pc.printf("left:  %f\n\r",biceps_l);
        }

    /*pc.printf("EMG right = %f\n\r",inRout);
    pc.printf("EMG left =  %f\n\r",inLout);
    pc.printf("envelope EMG right = %f\n\r",envelopeR);
    pc.printf("envelope EMG left =  %f\n\r",envelopeL);*/
    
    scope.set(0, inRout);
    scope.set(1, inLout);
    scope.set(2, envelopeR);
    scope.set(3, envelopeL);
    scope.set(4, Tout);
    
    
    scope.send();
    // To indicate that the function is working, the LED is toggled*/
    //led2 = !led2;
}


/*void sendValues( double outRenvelope, double outLenvelope){
    
    biceps_l = (double) outLenvelope * EMGgain; //emg0.read();     //velocity or reference position change, EMG with a gain
    biceps_r = (double) outRenvelope * EMGgain; //emg1.read();
    if (biceps_l > 0.2 && biceps_r > 0.2){
        //both arms activated: stamp moves down
        pc.printf("Stamp down\n\r");
        pc.printf("right: %f\n\r",biceps_r);
        pc.printf("left:  %f\n\r",biceps_l);
        //wait(0.5);
        led1=!led1;//blink purple
        led2=!led2;
        }
    else if (biceps_l > 0.2 && biceps_r <= 0.2){
        //arm 1 activated, move left
        pc.printf("Move left\n\r");
        pc.printf("right: %f\n\r",biceps_r);
        pc.printf("left:  %f\n\r",biceps_l);
        //wait(0.5);
        led2=1;//off
        led1=0;//on    red
        }
    else if (biceps_l <= 0.2 && biceps_r > 0.2){
        //arm 1 activated, move right
        pc.printf("Move right\n\r");
        pc.printf("right: %f\n\r",biceps_r);
        pc.printf("left:  %f\n\r",biceps_l);
        //wait(0.5);
        led2=0;//on    blue
        led1=1;//off
        }
    else{
        wait(0.2);
        led1 = 1;
        led2 = 1;  //off
        pc.printf("Nothing...\n\r");
        //wait(0.5);
        }
    // To indicate that the function is working, the LED is toggled
    //led2 = !led2; // blue
    
}*/



int main()
{   
    //pc.baud(SERIAL_BAUD);
    ledRed=1;
    ledBlue=1; 
    ledRed=0; //red

    bcq1R.add(&bq1R).add(&bq2R);
    bcq2R.add(&bq3R);
    
    bcq1L.add(&bq1L).add(&bq2L);
    bcq2L.add(&bq3L);
      
    filter_timer.attach(&filter_timer_act, 0.0004); //2500Hz (same as with filter coefficients on matlab!!! Thus adjust!)
    //send_timer.attach(&send_timer_act, 0.0004);
    //pc.printf("\rMain-loop\n\r");
        
        while(1)
        {
        if (filter_timer_go){
                filter_timer_go=false;
                FilteredSample(T);}
        /*if (send_timer_go){
                send_timer_go=false;
                sendValues(outRenvelope, outLenvelope);}*/
        }
}