Ömer Sakar / Mbed 2 deprecated KUT_HIDSCOPE_OLD_OLD

Het is kut

Dependencies:   HIDScope biquadFilter mbed

Fork of KUT_HIDSCOPE by Jan Boerman

main.cpp@4:c8bab10bc897, 2016-11-01 (annotated)

Committer:
ofosakar
Date:
Tue Nov 01 14:26:40 2016 +0000
Revision:
4:c8bab10bc897
Parent:
3:d4b8692e6697 
First working version of EMG processing

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
ofosakar 4:c8bab10bc897 11 BiQuadChain bqc;
ofosakar 4:c8bab10bc897 12 BiQuad bq1( 9.93756e-01, -1.89024e+00, 9.93756e-01, -1.89024e+00, 9.87512e-01 );
ofosakar 4:c8bab10bc897 13
Jankoekenpan 1:d357a1e80389 14 //Bandpass butterworth filter + Notch butterworth filter.
Jankoekenpan 1:d357a1e80389 15 //Bandpass: 10 --- 500 Hz
Jankoekenpan 1:d357a1e80389 16 //Nothc: 50 +- 2 Hz
Jankoekenpan 1:d357a1e80389 17 //BiQuad bq1( 4.97164e-02, 9.94328e-02, 4.97164e-02, -3.04503e-01, 6.36149e-02 );
Jankoekenpan 1:d357a1e80389 18 //BiQuad bq2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -3.87549e-01, 4.72280e-01 );
Jankoekenpan 1:d357a1e80389 19 //BiQuad bq3( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.95093e+00, 9.51645e-01 );
Jankoekenpan 1:d357a1e80389 20 //BiQuad bq4( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.97996e+00, 9.80645e-01 );
Jankoekenpan 1:d357a1e80389 21 //BiQuad bq5( 9.97389e-01, -1.97771e+00, 9.97389e-01, -1.97771e+00, 9.94778e-01 );
Jankoekenpan 1:d357a1e80389 22
Jankoekenpan 2:ef6c30f459a5 23 volatile float TOTAL_SAMPLE_SUM = 0;
Jankoekenpan 2:ef6c30f459a5 24 volatile long NUMBER_SAMPLES = 0;
Jankoekenpan 2:ef6c30f459a5 25
Jankoekenpan 1:d357a1e80389 26 const int numEmgCache = 50;
Jankoekenpan 1:d357a1e80389 27 float emgCache[numEmgCache]; //sorted from new to old;
Jankoekenpan 1:d357a1e80389 28
Jankoekenpan 1:d357a1e80389 29 void addFirst(float newValue, float array[], int size) {
Jankoekenpan 1:d357a1e80389 30 for (int i = size - 2; i >= 0; i--) {
Jankoekenpan 1:d357a1e80389 31 array[i+1] = array[i];
Jankoekenpan 1:d357a1e80389 32 }
Jankoekenpan 1:d357a1e80389 33 array[0] = newValue;
Jankoekenpan 1:d357a1e80389 34 }
Jankoekenpan 1:d357a1e80389 35
ofosakar 4:c8bab10bc897 36 float average(float newValue) {
ofosakar 4:c8bab10bc897 37 float sum = 0;
ofosakar 4:c8bab10bc897 38 for (int i = numEmgCache - 2; i >= 0; i--) {
ofosakar 4:c8bab10bc897 39 sum += emgCache[i];
ofosakar 4:c8bab10bc897 40 }
ofosakar 4:c8bab10bc897 41 // emgCache[0] = newValue;
ofosakar 4:c8bab10bc897 42 sum += newValue;
ofosakar 4:c8bab10bc897 43 return sum / numEmgCache;
ofosakar 4:c8bab10bc897 44 }
ofosakar 4:c8bab10bc897 45
Jankoekenpan 1:d357a1e80389 46 //shifts the array by adding the new emg value up front.
Jankoekenpan 1:d357a1e80389 47 //returns the new calculated average
Jankoekenpan 1:d357a1e80389 48 float movingAverage(float newValue) {
Jankoekenpan 1:d357a1e80389 49 float sum = 0;
Jankoekenpan 1:d357a1e80389 50 for (int i = numEmgCache - 2; i >= 0; i--) {
Jankoekenpan 1:d357a1e80389 51 emgCache[i+1] = emgCache[i];
Jankoekenpan 1:d357a1e80389 52 sum += emgCache[i];
Jankoekenpan 1:d357a1e80389 53 }
Jankoekenpan 1:d357a1e80389 54 emgCache[0] = newValue;
Jankoekenpan 1:d357a1e80389 55 sum += newValue;
Jankoekenpan 1:d357a1e80389 56 return sum / numEmgCache;
Jankoekenpan 1:d357a1e80389 57 }
Jankoekenpan 1:d357a1e80389 58
Jankoekenpan 1:d357a1e80389 59 float sum(float array[], int size) {
Jankoekenpan 1:d357a1e80389 60 float sum = 0;
Jankoekenpan 1:d357a1e80389 61 for (int i = 0; i < size; i++) {
Jankoekenpan 1:d357a1e80389 62 sum += array[i];
Jankoekenpan 1:d357a1e80389 63 }
Jankoekenpan 1:d357a1e80389 64 return sum;
Jankoekenpan 1:d357a1e80389 65 }
Jankoekenpan 1:d357a1e80389 66
Jankoekenpan 1:d357a1e80389 67 float mean(float array[], int size) {
Jankoekenpan 1:d357a1e80389 68 return sum(array, size) / size;
Jankoekenpan 1:d357a1e80389 69 }
Jankoekenpan 1:d357a1e80389 70
ofosakar 3:d4b8692e6697 71 float meanSquare(float array[], int size) {
ofosakar 4:c8bab10bc897 72 float naam[size];
ofosakar 3:d4b8692e6697 73 for(int i = 0; i < size; i++) {
ofosakar 4:c8bab10bc897 74 naam[i] = pow(array[i], 2);
ofosakar 3:d4b8692e6697 75 }
ofosakar 4:c8bab10bc897 76 return sum(naam, size) / size;
ofosakar 3:d4b8692e6697 77 }
ofosakar 3:d4b8692e6697 78
Jankoekenpan 2:ef6c30f459a5 79 float variance(float array[], int size, float avg) {
Jankoekenpan 1:d357a1e80389 80 float squaredDifferences[size];
Jankoekenpan 1:d357a1e80389 81 for (int i = 0; i < size; i++) {
Jankoekenpan 1:d357a1e80389 82 float difference = array[i] - avg;
Jankoekenpan 1:d357a1e80389 83 squaredDifferences[i] = difference*difference;
Jankoekenpan 1:d357a1e80389 84 }
Jankoekenpan 1:d357a1e80389 85 return mean(squaredDifferences, size);
Jankoekenpan 1:d357a1e80389 86 }
Jankoekenpan 1:d357a1e80389 87
Jankoekenpan 2:ef6c30f459a5 88 float standardDeviation(float array[], int size, float avg) {
Jankoekenpan 2:ef6c30f459a5 89 return sqrt(variance(array, size, avg));
Jankoekenpan 1:d357a1e80389 90 }
Jankoekenpan 1:d357a1e80389 91
Jankoekenpan 2:ef6c30f459a5 92 int decide(float value, float threshold) {
Jankoekenpan 2:ef6c30f459a5 93 return value < threshold ? 0 : 1;
Jankoekenpan 1:d357a1e80389 94 }
Jankoekenpan 0:e7f5d0b44e22 95
ofosakar 4:c8bab10bc897 96 float rectifier(float value, float avg) {
ofosakar 4:c8bab10bc897 97 return fabs(value - 0.5)*2;
ofosakar 4:c8bab10bc897 98 //return (avg + fabs(value - avg));
ofosakar 4:c8bab10bc897 99 }
Jankoekenpan 0:e7f5d0b44e22 100
Jankoekenpan 0:e7f5d0b44e22 101 void tick() {
Jankoekenpan 1:d357a1e80389 102 float emg = emg1.read();
Jankoekenpan 1:d357a1e80389 103 scope.set(0, emg);
ofosakar 4:c8bab10bc897 104
ofosakar 4:c8bab10bc897 105 float avg = average(fabs(emg));
ofosakar 4:c8bab10bc897 106
ofosakar 4:c8bab10bc897 107 float rect = rectifier(emg, avg);
ofosakar 4:c8bab10bc897 108 scope.set(1, rect);
ofosakar 4:c8bab10bc897 109
ofosakar 4:c8bab10bc897 110 float filtered = movingAverage( rect );
ofosakar 4:c8bab10bc897 111 scope.set(2, filtered);
ofosakar 4:c8bab10bc897 112
ofosakar 4:c8bab10bc897 113
ofosakar 4:c8bab10bc897 114 // filtered = movingAverage( fabs(emg) );
ofosakar 4:c8bab10bc897 115 // scope.set(3, filtered);
Jankoekenpan 1:d357a1e80389 116
Jankoekenpan 2:ef6c30f459a5 117 TOTAL_SAMPLE_SUM += emg;
ofosakar 4:c8bab10bc897 118 NUMBER_SAMPLES++;
Jankoekenpan 1:d357a1e80389 119
ofosakar 4:c8bab10bc897 120 // addFirst(filtered, averageCache, numAvgCache);
ofosakar 4:c8bab10bc897 121 // float avg = TOTAL_SAMPLE_SUM / NUMBER_SAMPLES; //TODO calibrate this value.
ofosakar 4:c8bab10bc897 122 // float stdDev = standardDeviation(averageCache, numAvgCache, avg);
ofosakar 4:c8bab10bc897 123 // scope.set(2, stdDev);
ofosakar 4:c8bab10bc897 124 //
ofosakar 4:c8bab10bc897 125 // float blocksize = sizeof(emgCache);
ofosakar 4:c8bab10bc897 126 // float rmsWithoutSquareRoot = meanSquare(emgCache, blocksize);
ofosakar 4:c8bab10bc897 127 // float rms = sqrt(rmsWithoutSquareRoot);
ofosakar 4:c8bab10bc897 128 // scope.set(3, rms);
ofosakar 3:d4b8692e6697 129
ofosakar 3:d4b8692e6697 130
ofosakar 4:c8bab10bc897 131 //float notch = bqc.step( emg );
ofosakar 4:c8bab10bc897 132 int threshold = decide(filtered ,0.2);
ofosakar 4:c8bab10bc897 133 scope.set(3, threshold);
ofosakar 4:c8bab10bc897 134 //float rectified =
ofosakar 4:c8bab10bc897 135
Jankoekenpan 1:d357a1e80389 136
Jankoekenpan 1:d357a1e80389 137 //scope.set(2, decide(filtered, 0.2f));
Jankoekenpan 1:d357a1e80389 138 //scope.set(3, decide(filtered, 0.3f));
Jankoekenpan 1:d357a1e80389 139 //scope.set(4, decide(filtered, 0.4f));
Jankoekenpan 1:d357a1e80389 140 //scope.set(5, decide(filtered, 0.5f));
Jankoekenpan 1:d357a1e80389 141
Jankoekenpan 0:e7f5d0b44e22 142 scope.send();
Jankoekenpan 0:e7f5d0b44e22 143 }
Jankoekenpan 0:e7f5d0b44e22 144
Jankoekenpan 0:e7f5d0b44e22 145 int main()
Jankoekenpan 0:e7f5d0b44e22 146 {
Jankoekenpan 1:d357a1e80389 147 //bqc.add( &bq1 ).add( &bq2 );//.add( &bq3 ).add( &bq4 ).add( &bq5 );
ofosakar 4:c8bab10bc897 148 bqc.add( &bq1 );
ofosakar 4:c8bab10bc897 149 ticker.attach(&tick, 0.002);
Jankoekenpan 0:e7f5d0b44e22 150 while (true);
Jankoekenpan 0:e7f5d0b44e22 151 }