Diff: emg-utils.cpp

Revision:

0:44d3f99b08c1

Child:

1:984b6b6812c7

diff -r 000000000000 -r 44d3f99b08c1 emg-utils.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/emg-utils.cpp	Wed Nov 02 11:08:53 2016 +0000
@@ -0,0 +1,157 @@
+#include "mbed.h"
+#include "HIDScope.h"
+#include "BiQuad.h"
+#include "math.h"
+#include "emg-utils.h"
+
+
+AnalogIn emg1(A0);
+AnalogIn emg2(A1);
+
+HIDScope scope(6);
+Ticker ticker;
+
+BiQuadChain bqc1;
+BiQuadChain bqc2;
+
+// EMG BIQUAD 1
+//Bandpass butterworth filter + Notch butterworth filter.
+//Bandpass: 10 --- 500 Hz
+//No Bandpass filters
+//Nothc:    50 +- 2 Hz
+BiQuad bq11( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
+
+// EMG BIQUAD 2
+//Bandpass butterworth filter + Notch butterworth filter.
+//Bandpass: 10 --- 500 Hz
+//No Bandpass filters
+//Nothc:    50 +- 2 Hz
+BiQuad bq12( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
+
+
+volatile float TOTAL_SAMPLE_SUM = 0;
+volatile long NUMBER_SAMPLES = 0;
+
+const int numEmgCache = 50;
+float emgCache1[numEmgCache]; //sorted from new to old;
+float emgCache2[numEmgCache]; //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;
+}
+
+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 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;
+}
+
+float variance(float array[], int size, float avg) {  
+    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, float avg) {
+    return sqrt(variance(array, size, avg));
+}
+
+int decide(float value, float threshold) {
+    return value < threshold ? 0 : 1;
+}
+
+float rectifier(float value) {
+    return fabs(value - 0.5f)*2.0f;
+}
+
+void processEMG() {
+    float emgOne = emg1.read();
+    scope.set(0, emgOne);
+    float notch1 = bqc1.step( emgOne );  
+    
+    float emgTwo = emg2.read();
+    scope.set(1, emgTwo);
+    float notch2 = bqc2.step( emgTwo );  
+    
+    
+
+    float rect1 = rectifier(notch1);
+//    scope.set(2, rect1);
+
+    float rect2 = rectifier(notch2);
+//    scope.set(3, rect2);    
+    
+    
+    float filtered1 = movingAverage( rect1, emgCache1, numEmgCache);
+    scope.set(2, filtered1);
+
+    float filtered2 = movingAverage( rect2, emgCache2, numEmgCache);
+    scope.set(3, filtered2);        
+
+    float threshold1 = 0.114318;
+    float threshold2 = 0.118785;
+    int decide1 = decide(filtered1 , threshold1);
+    scope.set(4, decide1);
+
+    int decide2 = decide(filtered2 , threshold2);
+    scope.set(5, decide2);
+        
+        
+    scope.send();
+}
+
+int main()
+{
+    bqc1.add( &bq11 );
+    bqc2.add( &bq12 );
+    
+
+//   bqc1.add( &bq11 );
+//   bqc2.add( &bq12 );
+   // 500 HZ Ticker
+   ticker.attach(&processEMG, 0.002);
+   while (true);
+}
\ No newline at end of file