File content as of revision 4:4de31fc4f912:

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

HIDScope scope(6);
// BUTTON USED IN CALIBRATION
DigitalIn calibrating(SW2);
// BUTTON TO START CALIBRATING
InterruptIn calibrateButton(SW3);
// THE TWO EMG SIGNALS
AnalogIn emg1(A0);
AnalogIn emg2(A1);
Serial pc(USBTX, USBRX);

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

//FOR DEBUG PURPOSES: WHEN TRUE, VALUES WILL BE WRITTEN TO HIDSCOPE
const bool printToHidscope = true;

// EMG BIQUAD 1
BiQuadChain bqc1;
BiQuadChain calibrateBqc1;
//Bandpass butterworth filter + Notch butterworth filter.
//No Bandpass filters
//Nothc:    50 +- 2 Hz
BiQuad calibrateBq11( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
BiQuad bq11( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );


// EMG BIQUAD 2
BiQuadChain bqc2;
BiQuadChain calibrateBqc2;
//Bandpass butterworth filter + Notch butterworth filter.
//Bandpass: 10 --- 500 Hz
//No Bandpass filters
//Nothc:    50 +- 2 Hz
BiQuad calibrateBq12( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
BiQuad bq12( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );

// ARRAYS USED IN CALIBRATING THE EMG SIGNALS
const int calibrateNumEmgCache = 100;
float calibrateEmgCache1[calibrateNumEmgCache]; //sorted from new to old;
float calibrateEmgCache2[calibrateNumEmgCache]; //sorted from new to old;

// ARRAYS USED IN CALCULATION OF THE MOVAG
// Values in these arrays contain samples that are already notched and rectified.
const int numEmgCache = 50;
float emgCache1[numEmgCache]; //sorted from new to old;
float emgCache2[numEmgCache]; //sorted from new to old;


// THRESHOLDS FOR THE DECISION: BY DEFAULT 0.2, 
// BUT SHOULD BE CHANGED IN THE CALIBRATION PHASE AT THE BEGINNING
volatile float threshold1 = 0.2;
volatile float threshold2 = 0.2;

// NUMBERS
int decided1[numEmgCache];
int decided2[numEmgCache];


Ticker ticker;
Ticker sampler;

float sample_frequency = 500.0f; //Hz
float Ts = 1.0f / sample_frequency;
// USED FOR COUNTING HOW MANY SIGNALS HAVE PASSED
volatile int count = 0;

// FUNC TO SEND THE DATA TO THE MOTOR
void (*motorFunc)(bool, bool);


////////////////////////////////////
///////// HELPER FUNCTIONS /////////
////////////////////////////////////
void resetLeds() {
    led_red = true;
    led_green = true;
    led_blue = true;    
}    

void addFirst(float newValue, float array[], int size) {
    for (int i = size - 2; i >= 0; i--) {
        array[i+1] = array[i];
    }
    array[0] = newValue;
}
void addFirst(int newValue, int array[], int size) {
    for (int i = size - 2; i >= 0; i--) {
        array[i+1] = array[i];
    }
    array[0] = newValue;
}

//shifts the array by adding the new emg value up front.
//returns the new calculated average
float movingAverage(float newValue, float array[], int size) {
    float sum = 0;
    for (int i = size - 2; i >= 0; i--) {
        array[i+1] = array[i];
        sum += array[i];
    }
    array[0] = newValue;
    sum += newValue;
    return sum / size;
}

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

float mean(float array[], int size) {
    return sum(array, size) / size;
}

float meanSquare(float array[], int size) {
    float naam[size];
    for(int i = 0; i < size; i++) {
        naam[i] = pow(array[i], 2);
    }
    return sum(naam, size) / size;
}

int decide(float value, float threshold) {
    return value < threshold ? 0 : 1;
}

float rectifier(float value) {
    return fabs(value - 0.5f)*2.0f;
}


void sendToMotor(void (*func)(bool, bool), bool arg1, bool arg2) {
    func(arg1, arg2);
}  
////////////////////////////////////
///////// HELPER FUNCTIONS /////////
////////////////////////////////////

void sample() {
    float emgOne = emg1.read();
    float notch1 = calibrateBqc1.step( emgOne );  
    
    float emgTwo = emg2.read();
    float notch2 = calibrateBqc2.step( emgTwo );  
    
    float rect1 = rectifier(notch1);
    float rect2 = rectifier(notch2);
    
    
    float filtered1 = movingAverage( rect1, calibrateEmgCache1, calibrateNumEmgCache);
    float filtered2 = movingAverage( rect2, calibrateEmgCache2, calibrateNumEmgCache);    
}

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 = mean(calibrateEmgCache1, calibrateNumEmgCache);
    float restAvg2 = mean(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 = mean(calibrateEmgCache1, calibrateNumEmgCache);
    float contAvg2 = mean(calibrateEmgCache2, calibrateNumEmgCache);
    
    threshold1 = (contAvg1 + restAvg1)/2;
    threshold2 = (contAvg2 + restAvg2)/2;    
    pc.printf("threshold1: %f\tthreshold2:%f\n\r", threshold1, threshold2);

}    
        
void processEMG() {
    float emgOne = emg1.read();
    float emgTwo = emg2.read();
    float notch1 = bqc1.step( emgOne );      
    float notch2 = bqc2.step( emgTwo );  
       
    float rect1 = rectifier(notch1);
    float rect2 = rectifier(notch2);
    
    float filtered1 = movingAverage( rect1, emgCache1, numEmgCache);
    float filtered2 = movingAverage( rect2, emgCache2, numEmgCache);

    int decide1 = decide(mean(emgCache1, numEmgCache ), threshold1);
    int decide2 = decide(mean(emgCache2, numEmgCache ), threshold2);
    addFirst(decide1, decided1, numEmgCache);
    addFirst(decide2, decided2, numEmgCache);

    if(printToHidscope) {
        scope.set(0,emgOne);
        scope.set(1,emgTwo);
        scope.set(2,decide1);
        scope.set(3,decide2);        
    }    

    if (count >= 49) {
        int counter1=0;
        int counter2=0;
        for(int i = 0; i < numEmgCache; ++i){
            if(decided1[i] == 0)
                ++counter1;
            if(decided2[i] == 0)
                ++counter2;        
        }
        int avgDecide1 = counter1 > std::ceil(numEmgCache/2.0) ? 0: 1;
        int avgDecide2 = counter2 > std::ceil(numEmgCache/2.0) ? 0: 1;
        sendToMotor(motorFunc,avgDecide1, avgDecide2);
        if(printToHidscope) {
            scope.set(4,avgDecide1);
            scope.set(5,avgDecide2);
        }    
        count =0;
    } else {
        count++;   
    }    
    scope.send();
}

void consumeBools(bool x, bool y) {
    pc.printf("%d\t%d\n\r", x, y);
} 
int main()
{
    pc.baud(115200);
    
    // initial state
    resetLeds();
    calibrateButton.fall(&calibrate);
    // TODO CHANGE THIS TO THE DESIERD FUNCTION (THAT JAN MADE)
    motorFunc = &consumeBools;

    // how to call the calibrating function    
    calibrate();

    
    bqc1.add( &bq11 );
    bqc2.add( &bq12 );
    
   // 500 HZ Ticker
   ticker.attach(&processEMG, 0.002);
   while (true);
}