Diff: calibrate.cpp

Revision:

2:8b790c03a760

Parent:

1:984b6b6812c7

Child:

3:082ba262d2ec

diff -r 984b6b6812c7 -r 8b790c03a760 calibrate.cpp
--- a/calibrate.cpp	Wed Nov 02 13:19:42 2016 +0000
+++ b/calibrate.cpp	Wed Nov 02 16:09:55 2016 +0000
@@ -1,107 +1,163 @@
 #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);
-AnalogIn calibrateEmg1(A0);
-AnalogIn calibrateEmg2(A1);
+// 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 );
 
 
-BiQuadChain calibrateBqc1;
+// EMG BIQUAD 2
+BiQuadChain bqc2;
 BiQuadChain calibrateBqc2;
-BiQuad calibrateBq11( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
+//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
+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;
-
-volatile float total1;
-volatile float total2;
-
-
-volatile float average1;
-volatile float average2;
+// USED FOR COUNTING HOW MANY SIGNALS HAVE PASSED
+volatile int count = 0;
 
-volatile bool isCalibrating;
+////////////////////////////////////
+///////// HELPER FUNCTIONS /////////
+////////////////////////////////////
+void resetLeds() {
+    led_red = true;
+    led_green = true;
+    led_blue = true;    
+}    
 
-float rectifierC(float value) {
-    return fabs(value - 0.5f)*2.0f;
+void addFirst(float newValue, float array[], int size) {
+    for (int i = size - 2; i >= 0; i--) {
+        array[i+1] = array[i];
+    }
+    array[0] = newValue;
 }
-float movingAverageC(float newValue, float array[], int size) {
-    float sumC = 0;
+void addFirst(int newValue, int array[], int size) {
     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 average(float newValue, float array[], int size) {
+    float sum = 0;
+    for (int i = size - 2; i >= 0; i--) {
+        sum += array[i];
+    }
+   // array[0] = newValue;
+    sum += newValue;
+    return sum / size;
+}
+
+//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 sumC(float array[], int size) {
-    float sumC = 0;
+float sum(float array[], int size) {
+    float sum = 0;
     for (int i = 0; i < size; i++) {
-        sumC += array[i];
+        sum += array[i];
     }
-    return sumC;
+    return sum;
+}
+
+float mean(float array[], int size) {
+    return sum(array, size) / size;
 }
 
-float meanC(float array[], int size) {
-    return sumC(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;
+}
+////////////////////////////////////
+///////// HELPER FUNCTIONS /////////
+////////////////////////////////////
+
 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 emgOne = emg1.read();
     float notch1 = calibrateBqc1.step( emgOne );  
     
-    float emgTwo = calibrateEmg2.read();
+    float emgTwo = emg2.read();
     float notch2 = calibrateBqc2.step( emgTwo );  
     
-    float rect1 = rectifierC(notch1);
-    float rect2 = rectifierC(notch2);
+    float rect1 = rectifier(notch1);
+    float rect2 = rectifier(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?
+    float filtered1 = movingAverage( rect1, calibrateEmgCache1, calibrateNumEmgCache);
+    float filtered2 = movingAverage( rect2, calibrateEmgCache2, calibrateNumEmgCache);    
 }
 
 void calibrate() {
@@ -120,8 +176,8 @@
     // 10 seconds sampled
     led_blue = true;
     sampler.detach();
-    float restAvg1 = meanC(calibrateEmgCache1, calibrateNumEmgCache);
-    float restAvg2 = meanC(calibrateEmgCache2, calibrateNumEmgCache);
+    float restAvg1 = mean(calibrateEmgCache1, calibrateNumEmgCache);
+    float restAvg2 = mean(calibrateEmgCache2, calibrateNumEmgCache);
     
     int i =0;
     while(i<3) {
@@ -158,31 +214,78 @@
         i++;
     }
     
-    float contAvg1 = meanC(calibrateEmgCache1, calibrateNumEmgCache);
-    float contAvg2 = meanC(calibrateEmgCache2, calibrateNumEmgCache);
+    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(average(filtered1, emgCache1, numEmgCache ), threshold1);
+    int decide2 = decide(average(filtered2, 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 >= 50) {
+        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;
+        if(printToHidscope) {
+            scope.set(4,avgDecide1);
+            scope.set(5,avgDecide2);
+        }    
+        count =0;
+    } else {
+        count++;   
+    }    
+    scope.send();
+}
+
 
 int main()
 {
     pc.baud(115200);
-    led_red = true;
-    led_green = true;
-    led_blue = true;
+    
+    // initial state
+    resetLeds();
     calibrateButton.fall(&calibrate);
-    
+
+    // how to call the calibrating function    
     calibrate();
-    pc.printf("threshold1: %f\tthreshold2:%f\n\r", threshold1, threshold2);
 
-//
-//    calibrateBqc1.add( &calibrateBq11 );
-//    calibrateBqc2.add( &calibrateBq12 );
-//        
-//    button.rise(&onRelease);
-//
-    while(true);
-}
\ No newline at end of file
+    
+    bqc1.add( &bq11 );
+    bqc2.add( &bq12 );
+    
+   // 500 HZ Ticker
+   ticker.attach(&processEMG, 0.002);
+   while (true);
+}