File content as of revision 0:18cb9a6c4fc1:

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

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

Ticker      filter_timer, send_timer;
DigitalOut  led1(LED_RED);
DigitalOut  led2(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;


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

BiQuadChain bcq1;
BiQuadChain bcq2;
// Notch filter wo=50; bw=wo/35
BiQuad bq1(9.9821e-01,-1.9807e+00,9.9821e-01,-1.9807e+00,9.9642e-01);
// High pass Butterworth filter 2nd order, Fc=10;
BiQuad bq2(9.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01);
// Low pass Butterworth filter 2nd order, Fc = 8;
BiQuad bq3(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(double &envelopeR, double &envelopeL)
{   
    double inLout = emg0.read();
    double inRout = emg1.read();
    
    double outRfilter1 = bcq1.step(inRout);
    double outRrect= fabs(outRfilter1);
    envelopeR = bcq2.step(outRrect);
    
    double outLfilter1 = bcq1.step(inLout);
    double outLrect = fabs(outLfilter1);
    envelopeL = bcq2.step(outLrect);

    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);
}

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);
    led1=1;
    led2=1; 
    led1=0; //red

    bcq1.add(&bq1).add(&bq2);
    bcq2.add(&bq3);
    
      
    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(outRenvelope, outLenvelope);}
        if (send_timer_go){
                send_timer_go=false;
                sendValues(outRenvelope, outLenvelope);}
        }
}