Diff: calibrate.cpp

Revision:

0:44d3f99b08c1

Child:

1:984b6b6812c7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/calibrate.cpp	Wed Nov 02 11:08:53 2016 +0000
@@ -0,0 +1,115 @@
+#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);
+//}
\ No newline at end of file