File content as of revision 21:30a33a4aa868:

#include "mbed.h"
#include "HIDScope.h"
#include "BiQuad.h"
#include "MovingAverage.h"
#include "MODSERIAL.h"
#include "math.h"
#define NSAMPLE 200


//Define objects
AnalogIn    emg0( A0 ); //Biceps Left
AnalogIn    emg1( A1 ); //Biceps Right
InterruptIn button(PTC6);
InterruptIn button2(PTA4);
Ticker      sample_timer;

//Ticker      driving_timer;
HIDScope    scope( 4 ); //Number of Channels
DigitalOut  led1(LED_GREEN);
DigitalOut  led2(LED_BLUE);
DigitalOut  led3(LED_RED);
MovingAverage <double>Movag_LB(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above 
MovingAverage <double>Movag_RB(NSAMPLE,0.0);

MODSERIAL pc(USBTX, USBRX);

volatile bool BicepLeft = false;
volatile bool BicepRight = false;


//Make Biquad filters Left(a0, a1, a2, b1, b2) 
BiQuadChain bqc1; //chain voor High Pass en Notch
BiQuad bq1(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
BiQuad bq2(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
BiQuad bq3(0.00044924972004654657, 0.0008984994400930931, 0.00044924972004654657, -1.6169385656964415, 0.6187355645766276); //Low Pass Filter

//Make Biquad filters Right
BiQuadChain bqc2; //chain voor High Pass en Notch
BiQuad bq4(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
BiQuad bq5(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter

//Function to read samples and filter and send to HIDScope
void sample()
{
    //Filter Left Bicep
    double LB_Filter_1 = bqc1.step(emg0.read()); //High Pass + Notch
    double LB_Rectify = fabs(LB_Filter_1); //Rectify Signal
    //double Filtered_emg_1 = bq3.step(LB_Rectify); //Low Pass 
    Movag_LB.Insert(LB_Rectify);
    double LB_Out = Movag_LB.GetAverage();
    
    //Filter Right Bicep
    double RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch
    double RB_Rectify = fabs(RB_Filter_1); //Rectify Signal
    //double Filtered_emg_1 = bq3.step(RB_Rectify); //Low Pass 
    Movag_RB.Insert(RB_Rectify);
    double RB_Out = Movag_RB.GetAverage();
    
    scope.set(0, emg0.read() ); //Raw EMG signal left bicep
    scope.set(1, LB_Out); // Filtered EMG signal left bicep
    scope.set(2, emg1.read()); //Raw EMG signal  right bicep
    scope.set(3, RB_Out); //Filtered EMG signal right bicep

    scope.send();

    if (LB_Out > ThresholdLeft) {
        led1 = 0;
        BicepLeft = true;
        }
    else {
        led1 = 1;
        BicepLeft = false;
        }
    if (RB_Out > ThresholdRight) {
        led2 = 0;
        BicepRight = true;
        }
    else {
        led2 = 1;
        BicepRight = false;
        }
}


int main()
{   
    bqc1.add( &bq1 ).add( &bq2 ); //make BiQuadChain
    bqc2.add( &bq4 ).add( &bq5 ); //make BiQuadChain
    sample_timer.attach(&sample, 0.002); //read samples at 500 Hz
    while(1) {}
}