hidscope herkent m'n device niet.

Dependencies:   HIDScope mbed biquadFilter

Committer:
Jankoekenpan
Date:
Tue Oct 25 14:12:40 2016 +0000
Revision:
1:d357a1e80389
Parent:
0:e7f5d0b44e22
Child:
2:ef6c30f459a5
implemented moving average and standard deviation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jankoekenpan 0:e7f5d0b44e22 1 #include "mbed.h"
Jankoekenpan 0:e7f5d0b44e22 2 #include "HIDScope.h"
Jankoekenpan 1:d357a1e80389 3 #include "BiQuad.h"
Jankoekenpan 1:d357a1e80389 4 #include "math.h"
Jankoekenpan 1:d357a1e80389 5
Jankoekenpan 1:d357a1e80389 6 AnalogIn emg1(A0);
Jankoekenpan 1:d357a1e80389 7
Jankoekenpan 1:d357a1e80389 8 HIDScope scope(6);
Jankoekenpan 1:d357a1e80389 9 Ticker ticker;
Jankoekenpan 1:d357a1e80389 10
Jankoekenpan 1:d357a1e80389 11 //BiQuadChain bqc;
Jankoekenpan 1:d357a1e80389 12 //Bandpass butterworth filter + Notch butterworth filter.
Jankoekenpan 1:d357a1e80389 13 //Bandpass: 10 --- 500 Hz
Jankoekenpan 1:d357a1e80389 14 //Nothc: 50 +- 2 Hz
Jankoekenpan 1:d357a1e80389 15 //BiQuad bq1( 4.97164e-02, 9.94328e-02, 4.97164e-02, -3.04503e-01, 6.36149e-02 );
Jankoekenpan 1:d357a1e80389 16 //BiQuad bq2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -3.87549e-01, 4.72280e-01 );
Jankoekenpan 1:d357a1e80389 17 //BiQuad bq3( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.95093e+00, 9.51645e-01 );
Jankoekenpan 1:d357a1e80389 18 //BiQuad bq4( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.97996e+00, 9.80645e-01 );
Jankoekenpan 1:d357a1e80389 19 //BiQuad bq5( 9.97389e-01, -1.97771e+00, 9.97389e-01, -1.97771e+00, 9.94778e-01 );
Jankoekenpan 1:d357a1e80389 20
Jankoekenpan 1:d357a1e80389 21 const int numEmgCache = 50;
Jankoekenpan 1:d357a1e80389 22 float emgCache[numEmgCache]; //sorted from new to old;
Jankoekenpan 1:d357a1e80389 23
Jankoekenpan 1:d357a1e80389 24 const int numAvgCache = 50;
Jankoekenpan 1:d357a1e80389 25 float averageCache[numAvgCache]; //sorted from new to old;
Jankoekenpan 1:d357a1e80389 26
Jankoekenpan 1:d357a1e80389 27 void addFirst(float newValue, float array[], int size) {
Jankoekenpan 1:d357a1e80389 28 for (int i = size - 2; i >= 0; i--) {
Jankoekenpan 1:d357a1e80389 29 array[i+1] = array[i];
Jankoekenpan 1:d357a1e80389 30 }
Jankoekenpan 1:d357a1e80389 31 array[0] = newValue;
Jankoekenpan 1:d357a1e80389 32 }
Jankoekenpan 1:d357a1e80389 33
Jankoekenpan 1:d357a1e80389 34 //shifts the array by adding the new emg value up front.
Jankoekenpan 1:d357a1e80389 35 //returns the new calculated average
Jankoekenpan 1:d357a1e80389 36 float movingAverage(float newValue) {
Jankoekenpan 1:d357a1e80389 37 float sum = 0;
Jankoekenpan 1:d357a1e80389 38 for (int i = numEmgCache - 2; i >= 0; i--) {
Jankoekenpan 1:d357a1e80389 39 emgCache[i+1] = emgCache[i];
Jankoekenpan 1:d357a1e80389 40 sum += emgCache[i];
Jankoekenpan 1:d357a1e80389 41 }
Jankoekenpan 1:d357a1e80389 42 emgCache[0] = newValue;
Jankoekenpan 1:d357a1e80389 43 sum += newValue;
Jankoekenpan 1:d357a1e80389 44 return sum / numEmgCache;
Jankoekenpan 1:d357a1e80389 45 }
Jankoekenpan 1:d357a1e80389 46
Jankoekenpan 1:d357a1e80389 47 float sum(float array[], int size) {
Jankoekenpan 1:d357a1e80389 48 float sum = 0;
Jankoekenpan 1:d357a1e80389 49 for (int i = 0; i < size; i++) {
Jankoekenpan 1:d357a1e80389 50 sum += array[i];
Jankoekenpan 1:d357a1e80389 51 }
Jankoekenpan 1:d357a1e80389 52 return sum;
Jankoekenpan 1:d357a1e80389 53 }
Jankoekenpan 1:d357a1e80389 54
Jankoekenpan 1:d357a1e80389 55 float mean(float array[], int size) {
Jankoekenpan 1:d357a1e80389 56 return sum(array, size) / size;
Jankoekenpan 1:d357a1e80389 57 }
Jankoekenpan 1:d357a1e80389 58
Jankoekenpan 1:d357a1e80389 59 float variance(float array[], int size) {
Jankoekenpan 1:d357a1e80389 60 float avg = mean(array, size);
Jankoekenpan 1:d357a1e80389 61 float squaredDifferences[size];
Jankoekenpan 1:d357a1e80389 62 for (int i = 0; i < size; i++) {
Jankoekenpan 1:d357a1e80389 63 float difference = array[i] - avg;
Jankoekenpan 1:d357a1e80389 64 squaredDifferences[i] = difference*difference;
Jankoekenpan 1:d357a1e80389 65 }
Jankoekenpan 1:d357a1e80389 66 return mean(squaredDifferences, size);
Jankoekenpan 1:d357a1e80389 67 }
Jankoekenpan 1:d357a1e80389 68
Jankoekenpan 1:d357a1e80389 69 float standardDeviation(float array[], int size) {
Jankoekenpan 1:d357a1e80389 70 return sqrt(variance(array, size));
Jankoekenpan 1:d357a1e80389 71 }
Jankoekenpan 1:d357a1e80389 72
Jankoekenpan 1:d357a1e80389 73 int decide(float signalvalue, float threshold) {
Jankoekenpan 1:d357a1e80389 74 return signalvalue < threshold ? 0 : 1;
Jankoekenpan 1:d357a1e80389 75 }
Jankoekenpan 0:e7f5d0b44e22 76
Jankoekenpan 0:e7f5d0b44e22 77
Jankoekenpan 0:e7f5d0b44e22 78 void tick() {
Jankoekenpan 1:d357a1e80389 79 float emg = emg1.read();
Jankoekenpan 1:d357a1e80389 80 scope.set(0, emg);
Jankoekenpan 1:d357a1e80389 81 float filtered = movingAverage( fabs(emg) );
Jankoekenpan 1:d357a1e80389 82 scope.set(1, filtered);
Jankoekenpan 1:d357a1e80389 83
Jankoekenpan 1:d357a1e80389 84 addFirst(filtered, averageCache, numAvgCache);
Jankoekenpan 1:d357a1e80389 85 float stdDev = standardDeviation(averageCache, numAvgCache);
Jankoekenpan 1:d357a1e80389 86 scope.set(2, stdDev);
Jankoekenpan 1:d357a1e80389 87
Jankoekenpan 1:d357a1e80389 88
Jankoekenpan 1:d357a1e80389 89 //float filtered = bqc.step( emg );
Jankoekenpan 1:d357a1e80389 90 //scope.set(1, filtered);
Jankoekenpan 1:d357a1e80389 91
Jankoekenpan 1:d357a1e80389 92 //scope.set(2, decide(filtered, 0.2f));
Jankoekenpan 1:d357a1e80389 93 //scope.set(3, decide(filtered, 0.3f));
Jankoekenpan 1:d357a1e80389 94 //scope.set(4, decide(filtered, 0.4f));
Jankoekenpan 1:d357a1e80389 95 //scope.set(5, decide(filtered, 0.5f));
Jankoekenpan 1:d357a1e80389 96
Jankoekenpan 0:e7f5d0b44e22 97 scope.send();
Jankoekenpan 0:e7f5d0b44e22 98 }
Jankoekenpan 0:e7f5d0b44e22 99
Jankoekenpan 0:e7f5d0b44e22 100 int main()
Jankoekenpan 0:e7f5d0b44e22 101 {
Jankoekenpan 1:d357a1e80389 102 //bqc.add( &bq1 ).add( &bq2 );//.add( &bq3 ).add( &bq4 ).add( &bq5 );
Jankoekenpan 1:d357a1e80389 103
Jankoekenpan 1:d357a1e80389 104 ticker.attach(&tick, 8.333333333333334e-4);
Jankoekenpan 0:e7f5d0b44e22 105 while (true);
Jankoekenpan 0:e7f5d0b44e22 106 }