File content as of revision 1:984b6b6812c7:

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

DigitalIn calibrating(SW2);
InterruptIn calibrateButton(SW3);
AnalogIn calibrateEmg1(A0);
AnalogIn calibrateEmg2(A1);
Serial pc(USBTX, USBRX);

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



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;

volatile float threshold1 = 0.2;
volatile float threshold2 = 0.2;

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?
}

void calibrate() {
    while(calibrating) {
        led_red = false;
        wait(0.5);    
        led_red = true;
        wait(0.5);    
    }
    
    // Button pressed for rest measurement
    led_red = true;
    sampler.attach(&sample, Ts);
    led_blue = false;
    wait(10);
    // 10 seconds sampled
    led_blue = true;
    sampler.detach();
    float restAvg1 = meanC(calibrateEmgCache1, calibrateNumEmgCache);
    float restAvg2 = meanC(calibrateEmgCache2, calibrateNumEmgCache);
    
    int i =0;
    while(i<3) {
        led_green = false;
        wait(0.5);    
        led_green = true;
        wait(0.5);    
        i++;
    }
    led_green = true;
    
    while(calibrating) {
        led_red = false;
        wait(0.5);    
        led_red = true;
        wait(0.5);    
    }
    // Button pressed for contracted measurement    
    led_red = true;
    sampler.attach(&sample, Ts);
    led_blue = false;
    wait(10);
    
    // 10 seconds sampled
    led_blue = true;
    sampler.detach();
    
    i =0;
    while(i<3) {
        led_green = false;
        wait(0.5);    
        led_green = true;
        wait(0.5);    
        i++;
    }
    
    float contAvg1 = meanC(calibrateEmgCache1, calibrateNumEmgCache);
    float contAvg2 = meanC(calibrateEmgCache2, calibrateNumEmgCache);
    
    threshold1 = (contAvg1 + restAvg1)/2;
    threshold2 = (contAvg2 + restAvg2)/2;    
    pc.printf("threshold1: %f\tthreshold2:%f\n\r", threshold1, threshold2);

}    

int main()
{
    pc.baud(115200);
    led_red = true;
    led_green = true;
    led_blue = true;
    calibrateButton.fall(&calibrate);
    
    calibrate();
    pc.printf("threshold1: %f\tthreshold2:%f\n\r", threshold1, threshold2);

//
//    calibrateBqc1.add( &calibrateBq11 );
//    calibrateBqc2.add( &calibrateBq12 );
//        
//    button.rise(&onRelease);
//
    while(true);
}