Pascal van Baardwijk / Mbed 2 deprecated EMGfilter

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope biquadFilter mbed

main.cpp@0:ae0bec143f2d, 2016-10-24 (annotated)

Committer:
pbaardwijk
Date:
Mon Oct 24 11:16:02 2016 +0000
Revision:
0:ae0bec143f2d
Child:
1:7fb4a74d33ff
EMG filter with normalization Nahuel approach

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pbaardwijk 0:ae0bec143f2d 1 #include "mbed.h"
pbaardwijk 0:ae0bec143f2d 2 #include "BiQuad.h"
pbaardwijk 0:ae0bec143f2d 3 #include "HIDScope.h"
pbaardwijk 0:ae0bec143f2d 4 //Enum with states
pbaardwijk 0:ae0bec143f2d 5 enum states {STATE_DEFAULT , STATE_CALIBRATION, STATE_RUN};
pbaardwijk 0:ae0bec143f2d 6
pbaardwijk 0:ae0bec143f2d 7 //Variable called 'state'
pbaardwijk 0:ae0bec143f2d 8 states state = STATE_DEFAULT;
pbaardwijk 0:ae0bec143f2d 9
pbaardwijk 0:ae0bec143f2d 10 //Creating two scope channels
pbaardwijk 0:ae0bec143f2d 11 HIDScope scope(2);
pbaardwijk 0:ae0bec143f2d 12
pbaardwijk 0:ae0bec143f2d 13 //Notch filter
pbaardwijk 0:ae0bec143f2d 14 BiQuadChain notch_50;
pbaardwijk 0:ae0bec143f2d 15 BiQuad bq1( 1.00000000000, -1.60956348896, 1.00000000000, -1.40195621505, 0.74203282402);
pbaardwijk 0:ae0bec143f2d 16 BiQuad bq2( 1.00000000000, -1.60724786352, 1.00000000000, -1.33646101015, 0.85967899264);
pbaardwijk 0:ae0bec143f2d 17 BiQuad bq3( 1.00000000000, -1.61186693071, 1.00000000000, -1.64415455961, 0.89726621230);
pbaardwijk 0:ae0bec143f2d 18
pbaardwijk 0:ae0bec143f2d 19 //High pass filter
pbaardwijk 0:ae0bec143f2d 20 BiQuadChain high_pass;
pbaardwijk 0:ae0bec143f2d 21 BiQuad bq4( 1.00000000000, -1.99999967822, 1.00000000000, -1.98388291862, 0.98395921205);
pbaardwijk 0:ae0bec143f2d 22 BiQuad bq5( 1.00000000000, -1.99999812453, 1.00000000000, -1.99324612474, 0.99332432675);
pbaardwijk 0:ae0bec143f2d 23
pbaardwijk 0:ae0bec143f2d 24 //Ticker
pbaardwijk 0:ae0bec143f2d 25 Ticker emgSampleTicker;
pbaardwijk 0:ae0bec143f2d 26
pbaardwijk 0:ae0bec143f2d 27 //Timeout to change state after 5 seconds
pbaardwijk 0:ae0bec143f2d 28 Timeout change_state;
pbaardwijk 0:ae0bec143f2d 29
pbaardwijk 0:ae0bec143f2d 30 //Timeout to change state after 10 seconds
pbaardwijk 0:ae0bec143f2d 31 Timeout change_state2;
pbaardwijk 0:ae0bec143f2d 32
pbaardwijk 0:ae0bec143f2d 33 //Emg input
pbaardwijk 0:ae0bec143f2d 34 AnalogIn emg0( A0 );
pbaardwijk 0:ae0bec143f2d 35 AnalogIn emg1( A1 );
pbaardwijk 0:ae0bec143f2d 36 AnalogIn emg2( A2 );
pbaardwijk 0:ae0bec143f2d 37
pbaardwijk 0:ae0bec143f2d 38 bool go_emgSample;
pbaardwijk 0:ae0bec143f2d 39 bool go_find_minmax;
pbaardwijk 0:ae0bec143f2d 40 double emg_sample[3];
pbaardwijk 0:ae0bec143f2d 41 double emg_notch[3];
pbaardwijk 0:ae0bec143f2d 42 double emg_high_passed[3];
pbaardwijk 0:ae0bec143f2d 43 double emg_rectified;
pbaardwijk 0:ae0bec143f2d 44 double min_emg[3];
pbaardwijk 0:ae0bec143f2d 45 double max_emg[3];
pbaardwijk 0:ae0bec143f2d 46
pbaardwijk 0:ae0bec143f2d 47 const int n = 200;
pbaardwijk 0:ae0bec143f2d 48 int counter = 0;
pbaardwijk 0:ae0bec143f2d 49 double RMSArray0[n] = {0};
pbaardwijk 0:ae0bec143f2d 50 double RMSArray1[n] = {0};
pbaardwijk 0:ae0bec143f2d 51 double RMSArray2[n] = {0};
pbaardwijk 0:ae0bec143f2d 52 double RMS0;
pbaardwijk 0:ae0bec143f2d 53 double RMS1;
pbaardwijk 0:ae0bec143f2d 54 double RMS2;
pbaardwijk 0:ae0bec143f2d 55 double SumRMS0;
pbaardwijk 0:ae0bec143f2d 56 double SumRMS1;
pbaardwijk 0:ae0bec143f2d 57 double SumRMS2;
pbaardwijk 0:ae0bec143f2d 58
pbaardwijk 0:ae0bec143f2d 59 double input_force0;
pbaardwijk 0:ae0bec143f2d 60 double input_force1;
pbaardwijk 0:ae0bec143f2d 61 double input_force2;
pbaardwijk 0:ae0bec143f2d 62
pbaardwijk 0:ae0bec143f2d 63 //count for emg min max
pbaardwijk 0:ae0bec143f2d 64 int start_calibration = 0;
pbaardwijk 0:ae0bec143f2d 65
pbaardwijk 0:ae0bec143f2d 66 void emgSample() {
pbaardwijk 0:ae0bec143f2d 67 go_emgSample = true;
pbaardwijk 0:ae0bec143f2d 68 }
pbaardwijk 0:ae0bec143f2d 69
pbaardwijk 0:ae0bec143f2d 70 void calibrate() {
pbaardwijk 0:ae0bec143f2d 71 state = STATE_CALIBRATION;
pbaardwijk 0:ae0bec143f2d 72 }
pbaardwijk 0:ae0bec143f2d 73
pbaardwijk 0:ae0bec143f2d 74 void run() {
pbaardwijk 0:ae0bec143f2d 75 state = STATE_RUN;
pbaardwijk 0:ae0bec143f2d 76 }
pbaardwijk 0:ae0bec143f2d 77
pbaardwijk 0:ae0bec143f2d 78 void EMG_filter();
pbaardwijk 0:ae0bec143f2d 79
pbaardwijk 0:ae0bec143f2d 80 int main() {
pbaardwijk 0:ae0bec143f2d 81 //combine biquads in biquad chains for notch/high- low-pass filters
pbaardwijk 0:ae0bec143f2d 82 notch_50.add( &bq1 ).add( &bq2 ).add( &bq3 );
pbaardwijk 0:ae0bec143f2d 83 high_pass.add( &bq4 ).add( &bq5 );
pbaardwijk 0:ae0bec143f2d 84
pbaardwijk 0:ae0bec143f2d 85 change_state.attach( &calibrate,5);
pbaardwijk 0:ae0bec143f2d 86 change_state2.attach( &run,10);
pbaardwijk 0:ae0bec143f2d 87 emgSampleTicker.attach( &emgSample, 0.002);
pbaardwijk 0:ae0bec143f2d 88 while( true ){
pbaardwijk 0:ae0bec143f2d 89 if(go_emgSample == true){
pbaardwijk 0:ae0bec143f2d 90 EMG_filter();
pbaardwijk 0:ae0bec143f2d 91 }
pbaardwijk 0:ae0bec143f2d 92 }
pbaardwijk 0:ae0bec143f2d 93 }
pbaardwijk 0:ae0bec143f2d 94
pbaardwijk 0:ae0bec143f2d 95
pbaardwijk 0:ae0bec143f2d 96 void EMG_filter() {
pbaardwijk 0:ae0bec143f2d 97 if(go_emgSample == true){
pbaardwijk 0:ae0bec143f2d 98 //read the emg signal
pbaardwijk 0:ae0bec143f2d 99 emg_sample[0] = emg0.read();
pbaardwijk 0:ae0bec143f2d 100 emg_sample[1] = emg1.read();
pbaardwijk 0:ae0bec143f2d 101 emg_sample[2] = emg2.read();
pbaardwijk 0:ae0bec143f2d 102
pbaardwijk 0:ae0bec143f2d 103 for (int i = 0; i < 3; i++){
pbaardwijk 0:ae0bec143f2d 104 //filter out the 50Hz components with a notch filter
pbaardwijk 0:ae0bec143f2d 105 //emg_notch[i] = notch_50.step(emg_sample[i]);
pbaardwijk 0:ae0bec143f2d 106
pbaardwijk 0:ae0bec143f2d 107 //high pass the signal (removing motion artifacts and offset)
pbaardwijk 0:ae0bec143f2d 108 emg_high_passed[i] = high_pass.step(emg_sample[i]);
pbaardwijk 0:ae0bec143f2d 109 }
pbaardwijk 0:ae0bec143f2d 110
pbaardwijk 0:ae0bec143f2d 111 //Calculating RMS
pbaardwijk 0:ae0bec143f2d 112 SumRMS0 -= pow(RMSArray0[counter],2);
pbaardwijk 0:ae0bec143f2d 113 SumRMS1 -= pow(RMSArray1[counter],2);
pbaardwijk 0:ae0bec143f2d 114 SumRMS2 -= pow(RMSArray2[counter],2);
pbaardwijk 0:ae0bec143f2d 115
pbaardwijk 0:ae0bec143f2d 116 RMSArray0[counter] = emg_high_passed[0];
pbaardwijk 0:ae0bec143f2d 117 RMSArray1[counter] = emg_high_passed[1];
pbaardwijk 0:ae0bec143f2d 118 RMSArray2[counter] = emg_high_passed[2];
pbaardwijk 0:ae0bec143f2d 119
pbaardwijk 0:ae0bec143f2d 120 SumRMS0 += pow(RMSArray0[counter],2);
pbaardwijk 0:ae0bec143f2d 121 SumRMS1 += pow(RMSArray1[counter],2);
pbaardwijk 0:ae0bec143f2d 122 SumRMS2 += pow(RMSArray2[counter],2);
pbaardwijk 0:ae0bec143f2d 123
pbaardwijk 0:ae0bec143f2d 124 counter++;
pbaardwijk 0:ae0bec143f2d 125 if (counter == n){
pbaardwijk 0:ae0bec143f2d 126 counter = 0;
pbaardwijk 0:ae0bec143f2d 127 }
pbaardwijk 0:ae0bec143f2d 128
pbaardwijk 0:ae0bec143f2d 129 RMS0 = sqrt(SumRMS0/n);
pbaardwijk 0:ae0bec143f2d 130 RMS1 = sqrt(SumRMS1/n);
pbaardwijk 0:ae0bec143f2d 131 RMS2 = sqrt(SumRMS2/n);
pbaardwijk 0:ae0bec143f2d 132
pbaardwijk 0:ae0bec143f2d 133 //Calculating min value and max value of emg signal
pbaardwijk 0:ae0bec143f2d 134 if(state == STATE_CALIBRATION)
pbaardwijk 0:ae0bec143f2d 135 {
pbaardwijk 0:ae0bec143f2d 136 if (start_calibration == 0) {
pbaardwijk 0:ae0bec143f2d 137 min_emg[0] = RMS0;
pbaardwijk 0:ae0bec143f2d 138 max_emg[0] = RMS0;
pbaardwijk 0:ae0bec143f2d 139 min_emg[1] = RMS1;
pbaardwijk 0:ae0bec143f2d 140 max_emg[1] = RMS1;
pbaardwijk 0:ae0bec143f2d 141 min_emg[2] = RMS2;
pbaardwijk 0:ae0bec143f2d 142 max_emg[2] = RMS2;
pbaardwijk 0:ae0bec143f2d 143 start_calibration++;
pbaardwijk 0:ae0bec143f2d 144 }
pbaardwijk 0:ae0bec143f2d 145 else {
pbaardwijk 0:ae0bec143f2d 146 //finding min and max of emg0
pbaardwijk 0:ae0bec143f2d 147 if (RMS0 < min_emg[0]) {
pbaardwijk 0:ae0bec143f2d 148 min_emg[0] = RMS0;
pbaardwijk 0:ae0bec143f2d 149 }
pbaardwijk 0:ae0bec143f2d 150 else if (RMS0 > max_emg[0]) {
pbaardwijk 0:ae0bec143f2d 151 max_emg[0] = RMS0;
pbaardwijk 0:ae0bec143f2d 152 }
pbaardwijk 0:ae0bec143f2d 153
pbaardwijk 0:ae0bec143f2d 154 //finding min and max of emg1
pbaardwijk 0:ae0bec143f2d 155 if (RMS1 < min_emg[1]) {
pbaardwijk 0:ae0bec143f2d 156 min_emg[1] = RMS1;
pbaardwijk 0:ae0bec143f2d 157 }
pbaardwijk 0:ae0bec143f2d 158 else if (RMS1 > max_emg[1]) {
pbaardwijk 0:ae0bec143f2d 159 max_emg[1] = RMS1;
pbaardwijk 0:ae0bec143f2d 160 }
pbaardwijk 0:ae0bec143f2d 161
pbaardwijk 0:ae0bec143f2d 162 //finding min and max of emg2
pbaardwijk 0:ae0bec143f2d 163 if (RMS2 < min_emg[2]) {
pbaardwijk 0:ae0bec143f2d 164 min_emg[2] = RMS2;
pbaardwijk 0:ae0bec143f2d 165 }
pbaardwijk 0:ae0bec143f2d 166 else if (RMS2 > max_emg[2]) {
pbaardwijk 0:ae0bec143f2d 167 max_emg[2] = RMS2;
pbaardwijk 0:ae0bec143f2d 168 }
pbaardwijk 0:ae0bec143f2d 169 }
pbaardwijk 0:ae0bec143f2d 170 }
pbaardwijk 0:ae0bec143f2d 171
pbaardwijk 0:ae0bec143f2d 172 //calculating input_forces for controller
pbaardwijk 0:ae0bec143f2d 173 input_force0 = (RMS0 - min_emg[0])/(max_emg[0]-min_emg[0]);
pbaardwijk 0:ae0bec143f2d 174 input_force1 = (RMS1 - min_emg[1])/(max_emg[1]-min_emg[1]);
pbaardwijk 0:ae0bec143f2d 175 input_force2 = (RMS2 - min_emg[2])/(max_emg[2]-min_emg[2]);
pbaardwijk 0:ae0bec143f2d 176
pbaardwijk 0:ae0bec143f2d 177 //Send scope data
pbaardwijk 0:ae0bec143f2d 178 scope.set(0,emg_sample[0]);
pbaardwijk 0:ae0bec143f2d 179 scope.set(1,input_force0);
pbaardwijk 0:ae0bec143f2d 180 //scope.set(2,input_force1);
pbaardwijk 0:ae0bec143f2d 181 //scope.set(3,input_force2);
pbaardwijk 0:ae0bec143f2d 182 scope.send();
pbaardwijk 0:ae0bec143f2d 183
pbaardwijk 0:ae0bec143f2d 184 go_emgSample = false;
pbaardwijk 0:ae0bec143f2d 185 }
pbaardwijk 0:ae0bec143f2d 186 }
pbaardwijk 0:ae0bec143f2d 187