Diff: main.cpp

Revision:

1:d357a1e80389

Parent:

0:e7f5d0b44e22

Child:

2:ef6c30f459a5

--- a/main.cpp	Thu Oct 20 10:20:58 2016 +0000
+++ b/main.cpp	Tue Oct 25 14:12:40 2016 +0000
@@ -1,20 +1,106 @@
 #include "mbed.h"
 #include "HIDScope.h"
+#include "BiQuad.h"
+#include "math.h"
+
+AnalogIn emg1(A0);
+
+HIDScope scope(6);
+Ticker ticker;
+
+//BiQuadChain bqc;
+//Bandpass butterworth filter + Notch butterworth filter.
+//Bandpass: 10 --- 500 Hz
+//Nothc:    50 +- 2 Hz
+//BiQuad bq1( 4.97164e-02, 9.94328e-02, 4.97164e-02, -3.04503e-01, 6.36149e-02 );
+//BiQuad bq2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -3.87549e-01, 4.72280e-01 );
+//BiQuad bq3( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.95093e+00, 9.51645e-01 );
+//BiQuad bq4( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.97996e+00, 9.80645e-01 );
+//BiQuad bq5( 9.97389e-01, -1.97771e+00, 9.97389e-01, -1.97771e+00, 9.94778e-01 );
+
+const int numEmgCache = 50;
+float emgCache[numEmgCache]; //sorted from new to old;
+
+const int numAvgCache = 50;
+float averageCache[numAvgCache]; //sorted from new to old;
+
+void addFirst(float newValue, float 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 sum = 0;
+    for (int i = numEmgCache - 2; i >= 0; i--) {
+        emgCache[i+1] = emgCache[i];
+        sum += emgCache[i];
+    }
+    emgCache[0] = newValue;
+    sum += newValue;
+    return sum / numEmgCache;
+}
+
+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 variance(float array[], int size) {
+    float avg = mean(array, size);    
+    float squaredDifferences[size];
+    for (int i = 0; i < size; i++) {
+        float difference = array[i] - avg;
+        squaredDifferences[i] = difference*difference;
+    }
+    return mean(squaredDifferences, size);
+}
+
+float standardDeviation(float array[], int size) {
+    return sqrt(variance(array, size));
+}
+
+int decide(float signalvalue, float threshold) {
+    return signalvalue < threshold ? 0 : 1;
+}
 
 
-AnalogIn emg1(A1);
-AnalogIn emg2(A2);
-HIDScope scope(1);
-Ticker ticker;
-
 void tick() {
-    scope.set(0, emg1.read());
-    scope.set(1, emg2.read());
+    float emg = emg1.read();
+    scope.set(0, emg);
+    float filtered = movingAverage( fabs(emg) );
+    scope.set(1, filtered);
+    
+    addFirst(filtered, averageCache, numAvgCache);
+    float stdDev = standardDeviation(averageCache, numAvgCache);
+    scope.set(2, stdDev);
+    
+    
+    //float filtered = bqc.step( emg );
+    //scope.set(1, filtered);
+    
+    //scope.set(2, decide(filtered, 0.2f));
+    //scope.set(3, decide(filtered, 0.3f));
+    //scope.set(4, decide(filtered, 0.4f));
+    //scope.set(5, decide(filtered, 0.5f));
+    
     scope.send();
 }
 
 int main()
 {
-   ticker.attach(&tick, 1/1200);
+   //bqc.add( &bq1 ).add( &bq2 );//.add( &bq3 ).add( &bq4 ).add( &bq5 );
+   
+   ticker.attach(&tick, 8.333333333333334e-4);
    while (true);
 }
\ No newline at end of file