Diff: main.cpp

Revision:

5:dd261ba7b047

Parent:

4:4de31fc4f912

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 03 10:01:14 2016 +0000
@@ -0,0 +1,275 @@
+#include "mbed.h"
+#include "BiQuad.h"
+// 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);
+
+// EMG BIQUAD 1
+BiQuadChain bqc1;
+//Notch iir filter.
+//Notch:    50 +- 2 Hz
+BiQuad bq11(9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
+
+
+// EMG BIQUAD 2
+BiQuadChain bqc2;
+//Notch iir filter.
+//Notch:    50 +- 2 Hz
+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 = bqc1.step( emgOne );  
+    
+    float emgTwo = emg2.read();
+    float notch2 = bqc2.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 (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);
+   
+        count =0;
+    } else {
+        count++;   
+    }    
+}
+
+void consumeBools(bool x, bool y) {
+    pc.printf("%d\t%d\r\n", x, y);
+} 
+int main()
+{
+    pc.baud(115200);
+    
+    // initial state
+    resetLeds();
+    
+    // initialize notch filters
+    bqc1.add( &bq11 );
+    bqc2.add( &bq12 );
+    
+    
+    calibrateButton.fall(&calibrate);
+    // TODO CHANGE THIS TO THE DESIERD FUNCTION (THAT JAN MADE)
+    motorFunc = &consumeBools;
+    
+
+    // call the calibrating function once at the start
+    // this function blocks until the calibration phase is over
+    calibrate();
+    
+   // 500 HZ Ticker
+   ticker.attach(&processEMG, Ts);
+   while (true);
+}