File content as of revision 0:44d3f99b08c1:

#include "mbed.h"
#include "BiQuad.h"

InterruptIn button(SW2);
AnalogIn calibrateEmg1(A0);
AnalogIn calibrateEmg2(A1);
Serial pc(USBTX, USBRX);

DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);


BiQuadChain calibrateBqc1;
BiQuadChain calibrateBqc2;
BiQuad calibrateBq11( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
BiQuad calibrateBq12( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );

const int calibrateNumEmgCache = 100;
float calibrateEmgCache1[calibrateNumEmgCache]; //sorted from new to old;
float calibrateEmgCache2[calibrateNumEmgCache]; //sorted from new to old;



Ticker sampler;

float sample_frequency = 500.0f; //Hz
float Ts = 1.0f / sample_frequency;

volatile float total1;
volatile float total2;


volatile float average1;
volatile float average2;

volatile bool isCalibrating;

float rectifierC(float value) {
    return fabs(value - 0.5f)*2.0f;
}
float movingAverageC(float newValue, float array[], int size) {
    float sumC = 0;
    for (int i = size - 2; i >= 0; i--) {
        array[i+1] = array[i];
        sumC += array[i];
    }
    array[0] = newValue;
    sumC += newValue;
    return sumC / size;
}

float sumC(float array[], int size) {
    float sumC = 0;
    for (int i = 0; i < size; i++) {
        sumC += array[i];
    }
    return sumC;
}

float meanC(float array[], int size) {
    return sumC(array, size) / size;
}

void sample() {
    
    //TODO apply filters and such.
    //Make use of EMG Processing library.
    //For now we will just sumC the raw emg signals
    
    float emgOne = calibrateEmg1.read();
    float notch1 = calibrateBqc1.step( emgOne );  
    
    float emgTwo = calibrateEmg2.read();
    float notch2 = calibrateBqc2.step( emgTwo );  
    
    float rect1 = rectifierC(notch1);
    float rect2 = rectifierC(notch2);
    
    
    float filtered1 = movingAverageC( rect1, calibrateEmgCache1, calibrateNumEmgCache);
    float filtered2 = movingAverageC( rect2, calibrateEmgCache2, calibrateNumEmgCache);

    
}

void onPress() {
    sampler.attach(&sample, Ts);
    led_red = true;
    led_green = false;
    
}

void onRelease() {
    led_red = false;
    led_green = true;
    sampler.detach();
    average1 = meanC(calibrateEmgCache1, calibrateNumEmgCache);
    average2 = meanC(calibrateEmgCache2, calibrateNumEmgCache);
    pc.printf ("(avg1, avg2) = (%f, %f)\r\n", average1, average2); //Why NaN? am I deviding by zero?
}

//int main()
//{
//    pc.baud(115200);
//    led_red = false;
//    led_green = true;
//
//    calibrateBqc1.add( &calibrateBq11 );
//    calibrateBqc2.add( &calibrateBq12 );
//        
//    button.rise(&onRelease);
//    button.fall(&onPress);
//
//    while(true);
//}