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@2:13fa37643b8a, 2016-10-24 (annotated)

Committer:
pbaardwijk
Date:
Mon Oct 24 22:08:52 2016 +0000
Revision:
2:13fa37643b8a
Parent:
1:7fb4a74d33ff 
Version with button presses to calibrate the system

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 1:7fb4a74d33ff 27 //LED
pbaardwijk 1:7fb4a74d33ff 28 DigitalOut led(LED_RED);
pbaardwijk 1:7fb4a74d33ff 29
pbaardwijk 2:13fa37643b8a 30 //Button
pbaardwijk 2:13fa37643b8a 31 InterruptIn button(SW2);
pbaardwijk 2:13fa37643b8a 32
pbaardwijk 2:13fa37643b8a 33 //Time led blink
pbaardwijk 2:13fa37643b8a 34 Timeout blink_timer;
pbaardwijk 2:13fa37643b8a 35
pbaardwijk 0:ae0bec143f2d 36 //Emg input
pbaardwijk 0:ae0bec143f2d 37 AnalogIn emg0( A0 );
pbaardwijk 0:ae0bec143f2d 38 AnalogIn emg1( A1 );
pbaardwijk 0:ae0bec143f2d 39 AnalogIn emg2( A2 );
pbaardwijk 0:ae0bec143f2d 40
pbaardwijk 0:ae0bec143f2d 41 bool go_emgSample;
pbaardwijk 0:ae0bec143f2d 42 bool go_find_minmax;
pbaardwijk 0:ae0bec143f2d 43 double emg_sample[3];
pbaardwijk 0:ae0bec143f2d 44 double emg_notch[3];
pbaardwijk 0:ae0bec143f2d 45 double emg_high_passed[3];
pbaardwijk 0:ae0bec143f2d 46 double emg_rectified;
pbaardwijk 0:ae0bec143f2d 47 double min_emg[3];
pbaardwijk 0:ae0bec143f2d 48 double max_emg[3];
pbaardwijk 0:ae0bec143f2d 49
pbaardwijk 0:ae0bec143f2d 50 const int n = 200;
pbaardwijk 0:ae0bec143f2d 51 int counter = 0;
pbaardwijk 0:ae0bec143f2d 52 double RMSArray0[n] = {0};
pbaardwijk 0:ae0bec143f2d 53 double RMSArray1[n] = {0};
pbaardwijk 0:ae0bec143f2d 54 double RMSArray2[n] = {0};
pbaardwijk 0:ae0bec143f2d 55 double RMS0;
pbaardwijk 0:ae0bec143f2d 56 double RMS1;
pbaardwijk 0:ae0bec143f2d 57 double RMS2;
pbaardwijk 0:ae0bec143f2d 58 double SumRMS0;
pbaardwijk 0:ae0bec143f2d 59 double SumRMS1;
pbaardwijk 0:ae0bec143f2d 60 double SumRMS2;
pbaardwijk 0:ae0bec143f2d 61
pbaardwijk 0:ae0bec143f2d 62 double input_force0;
pbaardwijk 0:ae0bec143f2d 63 double input_force1;
pbaardwijk 0:ae0bec143f2d 64 double input_force2;
pbaardwijk 0:ae0bec143f2d 65
pbaardwijk 0:ae0bec143f2d 66 //count for emg min max
pbaardwijk 0:ae0bec143f2d 67 int start_calibration = 0;
pbaardwijk 0:ae0bec143f2d 68
pbaardwijk 0:ae0bec143f2d 69 void emgSample() {
pbaardwijk 0:ae0bec143f2d 70 go_emgSample = true;
pbaardwijk 0:ae0bec143f2d 71 }
pbaardwijk 0:ae0bec143f2d 72
pbaardwijk 2:13fa37643b8a 73 void blink();
pbaardwijk 0:ae0bec143f2d 74
pbaardwijk 2:13fa37643b8a 75 void calibrate();
pbaardwijk 0:ae0bec143f2d 76
pbaardwijk 0:ae0bec143f2d 77 void EMG_filter();
pbaardwijk 0:ae0bec143f2d 78
pbaardwijk 0:ae0bec143f2d 79 int main() {
pbaardwijk 0:ae0bec143f2d 80 //combine biquads in biquad chains for notch/high- low-pass filters
pbaardwijk 0:ae0bec143f2d 81 notch_50.add( &bq1 ).add( &bq2 ).add( &bq3 );
pbaardwijk 0:ae0bec143f2d 82 high_pass.add( &bq4 ).add( &bq5 );
pbaardwijk 2:13fa37643b8a 83 button.mode(PullUp);
pbaardwijk 2:13fa37643b8a 84 button.rise(&calibrate);
pbaardwijk 1:7fb4a74d33ff 85 led.write(1);
pbaardwijk 0:ae0bec143f2d 86 emgSampleTicker.attach( &emgSample, 0.002);
pbaardwijk 0:ae0bec143f2d 87 while( true ){
pbaardwijk 0:ae0bec143f2d 88 if(go_emgSample == true){
pbaardwijk 0:ae0bec143f2d 89 EMG_filter();
pbaardwijk 0:ae0bec143f2d 90 }
pbaardwijk 0:ae0bec143f2d 91 }
pbaardwijk 0:ae0bec143f2d 92 }
pbaardwijk 0:ae0bec143f2d 93
pbaardwijk 0:ae0bec143f2d 94
pbaardwijk 0:ae0bec143f2d 95 void EMG_filter() {
pbaardwijk 0:ae0bec143f2d 96 if(go_emgSample == true){
pbaardwijk 0:ae0bec143f2d 97 //read the emg signal
pbaardwijk 0:ae0bec143f2d 98 emg_sample[0] = emg0.read();
pbaardwijk 0:ae0bec143f2d 99 emg_sample[1] = emg1.read();
pbaardwijk 0:ae0bec143f2d 100 emg_sample[2] = emg2.read();
pbaardwijk 0:ae0bec143f2d 101
pbaardwijk 0:ae0bec143f2d 102 for (int i = 0; i < 3; i++){
pbaardwijk 0:ae0bec143f2d 103 //filter out the 50Hz components with a notch filter
pbaardwijk 0:ae0bec143f2d 104 //emg_notch[i] = notch_50.step(emg_sample[i]);
pbaardwijk 0:ae0bec143f2d 105
pbaardwijk 0:ae0bec143f2d 106 //high pass the signal (removing motion artifacts and offset)
pbaardwijk 0:ae0bec143f2d 107 emg_high_passed[i] = high_pass.step(emg_sample[i]);
pbaardwijk 0:ae0bec143f2d 108 }
pbaardwijk 0:ae0bec143f2d 109
pbaardwijk 0:ae0bec143f2d 110 //Calculating RMS
pbaardwijk 0:ae0bec143f2d 111 SumRMS0 -= pow(RMSArray0[counter],2);
pbaardwijk 0:ae0bec143f2d 112 SumRMS1 -= pow(RMSArray1[counter],2);
pbaardwijk 0:ae0bec143f2d 113 SumRMS2 -= pow(RMSArray2[counter],2);
pbaardwijk 0:ae0bec143f2d 114
pbaardwijk 0:ae0bec143f2d 115 RMSArray0[counter] = emg_high_passed[0];
pbaardwijk 0:ae0bec143f2d 116 RMSArray1[counter] = emg_high_passed[1];
pbaardwijk 0:ae0bec143f2d 117 RMSArray2[counter] = emg_high_passed[2];
pbaardwijk 0:ae0bec143f2d 118
pbaardwijk 0:ae0bec143f2d 119 SumRMS0 += pow(RMSArray0[counter],2);
pbaardwijk 0:ae0bec143f2d 120 SumRMS1 += pow(RMSArray1[counter],2);
pbaardwijk 0:ae0bec143f2d 121 SumRMS2 += pow(RMSArray2[counter],2);
pbaardwijk 0:ae0bec143f2d 122
pbaardwijk 0:ae0bec143f2d 123 counter++;
pbaardwijk 0:ae0bec143f2d 124 if (counter == n){
pbaardwijk 0:ae0bec143f2d 125 counter = 0;
pbaardwijk 0:ae0bec143f2d 126 }
pbaardwijk 0:ae0bec143f2d 127
pbaardwijk 0:ae0bec143f2d 128 RMS0 = sqrt(SumRMS0/n);
pbaardwijk 0:ae0bec143f2d 129 RMS1 = sqrt(SumRMS1/n);
pbaardwijk 0:ae0bec143f2d 130 RMS2 = sqrt(SumRMS2/n);
pbaardwijk 0:ae0bec143f2d 131
pbaardwijk 0:ae0bec143f2d 132 //Calculating min value and max value of emg signal
pbaardwijk 2:13fa37643b8a 133 //if(state == STATE_CALIBRATION)
pbaardwijk 2:13fa37643b8a 134 //{
pbaardwijk 2:13fa37643b8a 135 // if (start_calibration == 0) {
pbaardwijk 2:13fa37643b8a 136 // min_emg[0] = RMS0;
pbaardwijk 2:13fa37643b8a 137 // max_emg[0] = RMS0;
pbaardwijk 2:13fa37643b8a 138 // min_emg[1] = RMS1;
pbaardwijk 2:13fa37643b8a 139 // max_emg[1] = RMS1;
pbaardwijk 2:13fa37643b8a 140 // min_emg[2] = RMS2;
pbaardwijk 2:13fa37643b8a 141 // max_emg[2] = RMS2;
pbaardwijk 2:13fa37643b8a 142 // start_calibration++;
pbaardwijk 2:13fa37643b8a 143 // }
pbaardwijk 2:13fa37643b8a 144 // else {
pbaardwijk 2:13fa37643b8a 145 // //finding min and max of emg0
pbaardwijk 2:13fa37643b8a 146 // if (RMS0 < min_emg[0]) {
pbaardwijk 2:13fa37643b8a 147 // min_emg[0] = RMS0;
pbaardwijk 2:13fa37643b8a 148 // }
pbaardwijk 2:13fa37643b8a 149 // else if (RMS0 > max_emg[0]) {
pbaardwijk 2:13fa37643b8a 150 // max_emg[0] = RMS0;
pbaardwijk 2:13fa37643b8a 151 // }
pbaardwijk 2:13fa37643b8a 152 //
pbaardwijk 2:13fa37643b8a 153 // //finding min and max of emg1
pbaardwijk 2:13fa37643b8a 154 // if (RMS1 < min_emg[1]) {
pbaardwijk 2:13fa37643b8a 155 // min_emg[1] = RMS1;
pbaardwijk 2:13fa37643b8a 156 // }
pbaardwijk 2:13fa37643b8a 157 // else if (RMS1 > max_emg[1]) {
pbaardwijk 2:13fa37643b8a 158 // max_emg[1] = RMS1;
pbaardwijk 2:13fa37643b8a 159 // }
pbaardwijk 2:13fa37643b8a 160 //
pbaardwijk 2:13fa37643b8a 161 // //finding min and max of emg2
pbaardwijk 2:13fa37643b8a 162 // if (RMS2 < min_emg[2]) {
pbaardwijk 2:13fa37643b8a 163 // min_emg[2] = RMS2;
pbaardwijk 2:13fa37643b8a 164 // }
pbaardwijk 2:13fa37643b8a 165 // else if (RMS2 > max_emg[2]) {
pbaardwijk 2:13fa37643b8a 166 // max_emg[2] = RMS2;
pbaardwijk 2:13fa37643b8a 167 // }
pbaardwijk 2:13fa37643b8a 168 // }
pbaardwijk 2:13fa37643b8a 169 //}
pbaardwijk 0:ae0bec143f2d 170
pbaardwijk 0:ae0bec143f2d 171 //calculating input_forces for controller
pbaardwijk 0:ae0bec143f2d 172 input_force0 = (RMS0 - min_emg[0])/(max_emg[0]-min_emg[0]);
pbaardwijk 0:ae0bec143f2d 173 input_force1 = (RMS1 - min_emg[1])/(max_emg[1]-min_emg[1]);
pbaardwijk 0:ae0bec143f2d 174 input_force2 = (RMS2 - min_emg[2])/(max_emg[2]-min_emg[2]);
pbaardwijk 0:ae0bec143f2d 175
pbaardwijk 0:ae0bec143f2d 176 //Send scope data
pbaardwijk 0:ae0bec143f2d 177 scope.set(0,emg_sample[0]);
pbaardwijk 0:ae0bec143f2d 178 scope.set(1,input_force0);
pbaardwijk 0:ae0bec143f2d 179 //scope.set(2,input_force1);
pbaardwijk 0:ae0bec143f2d 180 //scope.set(3,input_force2);
pbaardwijk 0:ae0bec143f2d 181 scope.send();
pbaardwijk 0:ae0bec143f2d 182
pbaardwijk 0:ae0bec143f2d 183 go_emgSample = false;
pbaardwijk 0:ae0bec143f2d 184 }
pbaardwijk 0:ae0bec143f2d 185 }
pbaardwijk 0:ae0bec143f2d 186
pbaardwijk 2:13fa37643b8a 187 void calibrate() {
pbaardwijk 2:13fa37643b8a 188 state = STATE_CALIBRATION;
pbaardwijk 2:13fa37643b8a 189 switch(start_calibration) {
pbaardwijk 2:13fa37643b8a 190 case 0 :
pbaardwijk 2:13fa37643b8a 191 break;
pbaardwijk 2:13fa37643b8a 192 case 1 :
pbaardwijk 2:13fa37643b8a 193 min_emg[0] = RMS0;
pbaardwijk 2:13fa37643b8a 194 break;
pbaardwijk 2:13fa37643b8a 195 case 2 :
pbaardwijk 2:13fa37643b8a 196 max_emg[0] = RMS0;
pbaardwijk 2:13fa37643b8a 197 break;
pbaardwijk 2:13fa37643b8a 198 case 3 :
pbaardwijk 2:13fa37643b8a 199 min_emg[1] = RMS1;
pbaardwijk 2:13fa37643b8a 200 break;
pbaardwijk 2:13fa37643b8a 201 case 4:
pbaardwijk 2:13fa37643b8a 202 max_emg[1] = RMS1;
pbaardwijk 2:13fa37643b8a 203 break;
pbaardwijk 2:13fa37643b8a 204 case 5:
pbaardwijk 2:13fa37643b8a 205 min_emg[2] = RMS2;
pbaardwijk 2:13fa37643b8a 206 break;
pbaardwijk 2:13fa37643b8a 207 case 6:
pbaardwijk 2:13fa37643b8a 208 max_emg[2] = RMS2;
pbaardwijk 2:13fa37643b8a 209 break;
pbaardwijk 2:13fa37643b8a 210 }
pbaardwijk 2:13fa37643b8a 211 if (start_calibration < 7){
pbaardwijk 2:13fa37643b8a 212 led.write(0);
pbaardwijk 2:13fa37643b8a 213 blink_timer.attach(&blink,0.5);
pbaardwijk 2:13fa37643b8a 214 start_calibration++;
pbaardwijk 2:13fa37643b8a 215 }
pbaardwijk 2:13fa37643b8a 216 }
pbaardwijk 2:13fa37643b8a 217
pbaardwijk 2:13fa37643b8a 218 void blink() {
pbaardwijk 2:13fa37643b8a 219 led.write(1);
pbaardwijk 2:13fa37643b8a 220 }