Rick Poppe / Mbed 2 deprecated EMG_filtering

the emg filtering part of the program

Dependencies:   HIDScope biquadFilter mbed MODSERIAL

Fork of EMG by Tom Tom

main.cpp@30:7cc564d38bc4, 2016-10-25 (annotated)

Committer:
RiP
Date:
Tue Oct 25 08:04:01 2016 +0000
Revision:
30:7cc564d38bc4
Parent:
29:ac08c1a32c54
Child:
31:9c3b022f1dc3
knopjes werken nu ook

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
RiP 21:3aecd735319d 3 #include "BiQuad.h"
RiP 22:f38a15e851d2 4 #include "MODSERIAL.h"
vsluiter 0:32bb76391d89 5
vsluiter 4:8b298dfada81 6 //Define objects
RiP 24:01b4b51b5dc6 7
RiP 24:01b4b51b5dc6 8 InterruptIn button_calibrate(PTA4);
RiP 21:3aecd735319d 9 AnalogIn emg1( A0 );
RiP 21:3aecd735319d 10 AnalogIn emg2( A1 );
RiP 21:3aecd735319d 11 AnalogIn emg3( A2 );
tomlankhorst 19:2bf824669684 12
RiP 27:1ff7fa636f1c 13 Ticker pos_timer;
tomlankhorst 14:f83354387756 14 Ticker sample_timer;
RiP 21:3aecd735319d 15 HIDScope scope( 6 );
RiP 22:f38a15e851d2 16 MODSERIAL pc(USBTX, USBRX);
tomlankhorst 18:21d8e7a81cf5 17 DigitalOut led(LED1);
vsluiter 2:e314bb3b2d99 18
RiP 22:f38a15e851d2 19 volatile bool sampletimer = false;
RiP 24:01b4b51b5dc6 20 volatile bool buttonflag = false;
RiP 25:1a71424b05ff 21 volatile bool newcase = false;
RiP 28:3b1b29193851 22 volatile bool newcaseprint = false;
RiP 29:ac08c1a32c54 23 volatile bool change_ref_timer = false;
RiP 25:1a71424b05ff 24
RiP 23:54d28f9eef53 25 double threshold = 0.04;
RiP 23:54d28f9eef53 26 double samplefreq=0.002;
RiP 25:1a71424b05ff 27 double emg02;
RiP 25:1a71424b05ff 28 double emg12;
RiP 25:1a71424b05ff 29 double emg22;
RiP 27:1ff7fa636f1c 30 double ref_x=0.000;
RiP 27:1ff7fa636f1c 31 double ref_y=0.000;
RiP 30:7cc564d38bc4 32 double speed=0.2;
RiP 27:1ff7fa636f1c 33
RiP 28:3b1b29193851 34 // create a variable called 'state', define it
RiP 23:54d28f9eef53 35 typedef enum { STATE_CALIBRATION, STATE_PAUZE, STATE_X, STATE_X_NEG, STATE_Y, STATE_Y_NEG, STATE_XY, STATE_XY_NEG } states;
RiP 22:f38a15e851d2 36
RiP 28:3b1b29193851 37 states mystate = STATE_PAUZE;
RiP 28:3b1b29193851 38
RiP 21:3aecd735319d 39 BiQuadChain bqc11;
RiP 21:3aecd735319d 40 BiQuadChain bqc13;
RiP 21:3aecd735319d 41 BiQuadChain bqc21;
RiP 21:3aecd735319d 42 BiQuadChain bqc23;
RiP 21:3aecd735319d 43 BiQuadChain bqc31;
RiP 21:3aecd735319d 44 BiQuadChain bqc33;
RiP 21:3aecd735319d 45 //BiQuad bq11( 9.87589e-01, -1.59795e+00, 9.87589e-01, -1.59795e+00, 9.75178e-01 ); //oude BiQuad waardes
RiP 21:3aecd735319d 46 /* BiQuads for filter emg1
RiP 21:3aecd735319d 47 notch filter*/
RiP 21:3aecd735319d 48 BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
RiP 21:3aecd735319d 49 BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
RiP 21:3aecd735319d 50 BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
RiP 21:3aecd735319d 51 /* High pass filter*/
RiP 21:3aecd735319d 52 BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 );
RiP 21:3aecd735319d 53 /* low pass filter*/
RiP 21:3aecd735319d 54 BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
RiP 21:3aecd735319d 55
RiP 21:3aecd735319d 56 /* BiQuads for filter emg2
RiP 21:3aecd735319d 57 notch filter*/
RiP 21:3aecd735319d 58 BiQuad bq211(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
RiP 21:3aecd735319d 59 BiQuad bq212(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
RiP 21:3aecd735319d 60 BiQuad bq213(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
RiP 21:3aecd735319d 61 /* High pass filter*/
RiP 21:3aecd735319d 62 BiQuad bq221( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 );
RiP 21:3aecd735319d 63 /* low pass filter*/
RiP 21:3aecd735319d 64 BiQuad bq231( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
RiP 21:3aecd735319d 65
RiP 21:3aecd735319d 66 /* BiQuads for filter emg3
RiP 21:3aecd735319d 67 notch filter*/
RiP 21:3aecd735319d 68 BiQuad bq311(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
RiP 21:3aecd735319d 69 BiQuad bq312(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
RiP 21:3aecd735319d 70 BiQuad bq313(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
RiP 21:3aecd735319d 71 /* High pass filter*/
RiP 21:3aecd735319d 72 BiQuad bq321( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 );
RiP 21:3aecd735319d 73 /* low pass filter*/
RiP 21:3aecd735319d 74 BiQuad bq331( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
RiP 21:3aecd735319d 75
RiP 22:f38a15e851d2 76
RiP 27:1ff7fa636f1c 77
RiP 22:f38a15e851d2 78 void sampleflag()
RiP 22:f38a15e851d2 79 {
RiP 22:f38a15e851d2 80 sampletimer=true;
RiP 29:ac08c1a32c54 81 change_ref_timer=true;
RiP 22:f38a15e851d2 82 }
RiP 22:f38a15e851d2 83
RiP 24:01b4b51b5dc6 84 void buttonflag_go()
RiP 24:01b4b51b5dc6 85 {
RiP 24:01b4b51b5dc6 86 buttonflag=true;
RiP 24:01b4b51b5dc6 87 }
RiP 22:f38a15e851d2 88
RiP 25:1a71424b05ff 89 void sample_button(states &mystate)
RiP 25:1a71424b05ff 90 {
RiP 25:1a71424b05ff 91 states myoldstate=mystate;
RiP 25:1a71424b05ff 92 char key=pc.getc();
RiP 25:1a71424b05ff 93
RiP 26:91d48c0b722d 94 // pc.printf("%c/n",key);
RiP 25:1a71424b05ff 95
RiP 25:1a71424b05ff 96 switch (key) {
RiP 25:1a71424b05ff 97 case 'p' : // run
RiP 25:1a71424b05ff 98 emg02=0.0;
RiP 25:1a71424b05ff 99 emg12=0.0;
RiP 25:1a71424b05ff 100 emg22=0.0;
RiP 25:1a71424b05ff 101 break;
RiP 25:1a71424b05ff 102 case 'q' : // run
RiP 25:1a71424b05ff 103 emg02=0.0;
RiP 25:1a71424b05ff 104 emg12=1.0;
RiP 25:1a71424b05ff 105 emg22=0.0;
RiP 25:1a71424b05ff 106 break;
RiP 25:1a71424b05ff 107 case 'w' : // run
RiP 25:1a71424b05ff 108 emg02=0.0;
RiP 25:1a71424b05ff 109 emg12=0.0;
RiP 25:1a71424b05ff 110 emg22=1.0;
RiP 25:1a71424b05ff 111 break;
RiP 25:1a71424b05ff 112 case 'e' : // run
RiP 25:1a71424b05ff 113 emg02=0.0;
RiP 25:1a71424b05ff 114 emg12=1.0;
RiP 25:1a71424b05ff 115 emg22=1.0;
RiP 25:1a71424b05ff 116 break;
RiP 25:1a71424b05ff 117 case 'a' : // run
RiP 25:1a71424b05ff 118 emg02=1.0;
RiP 25:1a71424b05ff 119 emg12=1.0;
RiP 25:1a71424b05ff 120 emg22=0.0;
RiP 25:1a71424b05ff 121 break;
RiP 25:1a71424b05ff 122 case 's' : // run
RiP 25:1a71424b05ff 123 emg02=1.0;
RiP 25:1a71424b05ff 124 emg12=0.0;
RiP 25:1a71424b05ff 125 emg22=1.0;
RiP 25:1a71424b05ff 126 break;
RiP 25:1a71424b05ff 127 case 'd' : // run
RiP 25:1a71424b05ff 128 emg02=1.0;
RiP 25:1a71424b05ff 129 emg12=1.0;
RiP 25:1a71424b05ff 130 emg22=1.0;
RiP 25:1a71424b05ff 131 break;
RiP 25:1a71424b05ff 132 }
RiP 25:1a71424b05ff 133
RiP 25:1a71424b05ff 134
RiP 25:1a71424b05ff 135
RiP 25:1a71424b05ff 136
RiP 25:1a71424b05ff 137
RiP 25:1a71424b05ff 138 led = !led;
RiP 25:1a71424b05ff 139 if (emg02>threshold) {
RiP 25:1a71424b05ff 140 if (emg12>threshold&&emg22>threshold) {
RiP 25:1a71424b05ff 141 mystate = STATE_XY_NEG;
RiP 25:1a71424b05ff 142 } else if (emg12>threshold) {
RiP 25:1a71424b05ff 143 mystate = STATE_X_NEG;
RiP 25:1a71424b05ff 144
RiP 25:1a71424b05ff 145 } else if (emg22>threshold) {
RiP 25:1a71424b05ff 146 mystate = STATE_Y_NEG;
RiP 25:1a71424b05ff 147 } else {
RiP 25:1a71424b05ff 148 mystate = STATE_PAUZE;
RiP 25:1a71424b05ff 149 }
RiP 25:1a71424b05ff 150 } else {
RiP 25:1a71424b05ff 151 if (emg12>threshold&&emg22>threshold) {
RiP 25:1a71424b05ff 152 mystate = STATE_XY;
RiP 25:1a71424b05ff 153 } else if (emg12>threshold) {
RiP 25:1a71424b05ff 154 mystate = STATE_X;
RiP 25:1a71424b05ff 155
RiP 25:1a71424b05ff 156 } else if (emg22>threshold) {
RiP 25:1a71424b05ff 157 mystate = STATE_Y;
RiP 25:1a71424b05ff 158 } else {
RiP 25:1a71424b05ff 159 mystate = STATE_PAUZE;
RiP 25:1a71424b05ff 160 }
RiP 25:1a71424b05ff 161 }
RiP 27:1ff7fa636f1c 162
RiP 25:1a71424b05ff 163 if (buttonflag==true) {
RiP 25:1a71424b05ff 164 mystate = STATE_CALIBRATION;
RiP 25:1a71424b05ff 165 }
RiP 25:1a71424b05ff 166
RiP 25:1a71424b05ff 167 if (myoldstate==mystate) {
RiP 25:1a71424b05ff 168 newcase=false;
RiP 28:3b1b29193851 169 newcaseprint=false;
RiP 28:3b1b29193851 170
RiP 25:1a71424b05ff 171 } else {
RiP 25:1a71424b05ff 172 newcase=true;
RiP 28:3b1b29193851 173 newcaseprint=true;
RiP 25:1a71424b05ff 174 }
RiP 25:1a71424b05ff 175 }
RiP 25:1a71424b05ff 176
RiP 23:54d28f9eef53 177 void sample(states &mystate)
vsluiter 2:e314bb3b2d99 178 {
RiP 27:1ff7fa636f1c 179 states myoldstate=mystate;
RiP 23:54d28f9eef53 180
RiP 21:3aecd735319d 181 /* Read the emg signals and filter it*/
RiP 22:f38a15e851d2 182
RiP 21:3aecd735319d 183 scope.set(0, emg1.read()); //original signal
RiP 27:1ff7fa636f1c 184 emg02=bqc13.step(fabs(bqc11.step(emg1.read())));
RiP 21:3aecd735319d 185 scope.set(1, emg02);
RiP 21:3aecd735319d 186 /* Read the second emg signal and filter it*/
RiP 21:3aecd735319d 187 scope.set(2, emg2.read()); //original signal
RiP 27:1ff7fa636f1c 188 emg12=bqc23.step(fabs(bqc21.step(emg2.read())));
RiP 21:3aecd735319d 189 scope.set(3, emg12);
RiP 21:3aecd735319d 190 /* Read the third emg signal and filter it*/
RiP 21:3aecd735319d 191 scope.set(4, emg3.read()); //original signal
RiP 27:1ff7fa636f1c 192 emg22=bqc33.step(fabs(bqc31.step(emg3.read())));
RiP 21:3aecd735319d 193 scope.set(5, emg22);
RiP 29:ac08c1a32c54 194
RiP 22:f38a15e851d2 195 //pc.printf("Hello World!\n");
RiP 21:3aecd735319d 196 /* Ensure that enough channels are available (HIDScope scope( 2 ))
RiP 21:3aecd735319d 197 * Finally, send all channels to the PC at once */
vsluiter 11:ce72ec658a95 198 scope.send();
RiP 21:3aecd735319d 199 /* To indicate that the function is working, the LED is toggled */
RiP 25:1a71424b05ff 200
tomlankhorst 18:21d8e7a81cf5 201 led = !led;
RiP 23:54d28f9eef53 202 if (emg02>threshold) {
RiP 23:54d28f9eef53 203 if (emg12>threshold&&emg22>threshold) {
RiP 24:01b4b51b5dc6 204 mystate = STATE_XY_NEG;
RiP 23:54d28f9eef53 205 } else if (emg12>threshold) {
RiP 24:01b4b51b5dc6 206 mystate = STATE_X_NEG;
RiP 23:54d28f9eef53 207
RiP 23:54d28f9eef53 208 } else if (emg22>threshold) {
RiP 24:01b4b51b5dc6 209 mystate = STATE_Y_NEG;
RiP 23:54d28f9eef53 210 } else {
RiP 24:01b4b51b5dc6 211 mystate = STATE_PAUZE;
RiP 23:54d28f9eef53 212 }
RiP 22:f38a15e851d2 213 } else {
RiP 23:54d28f9eef53 214 if (emg12>threshold&&emg22>threshold) {
RiP 24:01b4b51b5dc6 215 mystate = STATE_XY;
RiP 23:54d28f9eef53 216 } else if (emg12>threshold) {
RiP 24:01b4b51b5dc6 217 mystate = STATE_X;
RiP 23:54d28f9eef53 218
RiP 23:54d28f9eef53 219 } else if (emg22>threshold) {
RiP 24:01b4b51b5dc6 220 mystate = STATE_Y;
RiP 23:54d28f9eef53 221 } else {
RiP 24:01b4b51b5dc6 222 mystate = STATE_PAUZE;
RiP 23:54d28f9eef53 223 }
RiP 22:f38a15e851d2 224 }
RiP 23:54d28f9eef53 225
RiP 24:01b4b51b5dc6 226 if (buttonflag==true) {
RiP 24:01b4b51b5dc6 227 mystate = STATE_CALIBRATION;
RiP 24:01b4b51b5dc6 228 }
RiP 24:01b4b51b5dc6 229
RiP 27:1ff7fa636f1c 230 if (myoldstate==mystate) {
RiP 28:3b1b29193851 231 newcaseprint=false;
RiP 27:1ff7fa636f1c 232 newcase=false;
RiP 27:1ff7fa636f1c 233 } else {
RiP 28:3b1b29193851 234 newcaseprint=true;
RiP 27:1ff7fa636f1c 235 newcase=true;
RiP 27:1ff7fa636f1c 236 }
vsluiter 2:e314bb3b2d99 237 }
vsluiter 0:32bb76391d89 238
RiP 27:1ff7fa636f1c 239 void change_x(int direction)
RiP 27:1ff7fa636f1c 240 {
RiP 30:7cc564d38bc4 241 ref_x=ref_x+speed*direction;
RiP 27:1ff7fa636f1c 242
RiP 27:1ff7fa636f1c 243 }
RiP 27:1ff7fa636f1c 244
RiP 27:1ff7fa636f1c 245 void change_y(int direction)
RiP 27:1ff7fa636f1c 246 {
RiP 30:7cc564d38bc4 247 ref_y=ref_y+speed*direction;
RiP 27:1ff7fa636f1c 248
RiP 27:1ff7fa636f1c 249 }
RiP 27:1ff7fa636f1c 250 void change_xy(int direction)
RiP 27:1ff7fa636f1c 251 {
RiP 30:7cc564d38bc4 252 ref_x=ref_x+speed*direction;
RiP 30:7cc564d38bc4 253 ref_y=ref_y+speed*direction;
RiP 27:1ff7fa636f1c 254 }
RiP 27:1ff7fa636f1c 255
RiP 27:1ff7fa636f1c 256 void my_pos()
RiP 27:1ff7fa636f1c 257 {
RiP 27:1ff7fa636f1c 258 pc.printf("x_pos=%.4f\ty_pos=%.4f\n\r",ref_x,ref_y);
RiP 27:1ff7fa636f1c 259
RiP 27:1ff7fa636f1c 260 }
RiP 26:91d48c0b722d 261
RiP 29:ac08c1a32c54 262 void print_state()
RiP 28:3b1b29193851 263 {
RiP 28:3b1b29193851 264 // switch, case
RiP 28:3b1b29193851 265 if (newcaseprint==true) {
RiP 28:3b1b29193851 266 switch (mystate) {
RiP 28:3b1b29193851 267 case STATE_CALIBRATION : { // calibration
RiP 28:3b1b29193851 268 pc.printf("calibration\n\r");
RiP 28:3b1b29193851 269 break;
RiP 28:3b1b29193851 270 }
RiP 28:3b1b29193851 271 case STATE_X : // run
RiP 29:ac08c1a32c54 272 pc.printf("X\n\r");
RiP 28:3b1b29193851 273 break;
RiP 28:3b1b29193851 274 case STATE_X_NEG : // run
RiP 29:ac08c1a32c54 275 pc.printf("Xneg\n\r");
RiP 28:3b1b29193851 276 break;
RiP 28:3b1b29193851 277 case STATE_Y : // execute mode 1
RiP 29:ac08c1a32c54 278 pc.printf("Y\n\r");
RiP 28:3b1b29193851 279 break;
RiP 28:3b1b29193851 280 case STATE_Y_NEG : // execute mode 1
RiP 29:ac08c1a32c54 281 pc.printf("Yneg\n\r");
RiP 28:3b1b29193851 282 break;
RiP 28:3b1b29193851 283 case STATE_XY : // execute mode 2
RiP 29:ac08c1a32c54 284 pc.printf("XY\n\r");
RiP 28:3b1b29193851 285 break;
RiP 28:3b1b29193851 286 case STATE_XY_NEG : // execute mode 2
RiP 29:ac08c1a32c54 287 pc.printf("XYneg\n\r");
RiP 28:3b1b29193851 288 break;
RiP 28:3b1b29193851 289 case STATE_PAUZE : // default
RiP 29:ac08c1a32c54 290 pc.printf("PAUZE\n\r");
RiP 28:3b1b29193851 291 break;
RiP 28:3b1b29193851 292 }
RiP 28:3b1b29193851 293 }
RiP 28:3b1b29193851 294
RiP 28:3b1b29193851 295 }
RiP 28:3b1b29193851 296
RiP 29:ac08c1a32c54 297 void change_ref()
RiP 29:ac08c1a32c54 298 {
RiP 29:ac08c1a32c54 299 if (change_ref_timer==true) {
RiP 29:ac08c1a32c54 300 if (mystate == STATE_X) {
RiP 29:ac08c1a32c54 301 change_x(1);
RiP 29:ac08c1a32c54 302 } else if (mystate == STATE_X_NEG) {
RiP 29:ac08c1a32c54 303 change_x(-1);
RiP 29:ac08c1a32c54 304 } else if (mystate == STATE_Y) {
RiP 29:ac08c1a32c54 305 change_y(1);
RiP 29:ac08c1a32c54 306 } else if (mystate == STATE_Y_NEG) {
RiP 29:ac08c1a32c54 307 change_y(-1);
RiP 29:ac08c1a32c54 308 } else if (mystate == STATE_XY) {
RiP 29:ac08c1a32c54 309 change_xy(1);
RiP 29:ac08c1a32c54 310 } else if (mystate == STATE_XY_NEG) {
RiP 29:ac08c1a32c54 311 change_xy(-1);
RiP 29:ac08c1a32c54 312 }
RiP 29:ac08c1a32c54 313 change_ref_timer=false;
RiP 29:ac08c1a32c54 314 }
RiP 29:ac08c1a32c54 315 }
RiP 29:ac08c1a32c54 316
RiP 29:ac08c1a32c54 317
vsluiter 0:32bb76391d89 318 int main()
RiP 21:3aecd735319d 319 {
RiP 29:ac08c1a32c54 320 pc.printf("RESET\n\r");
RiP 22:f38a15e851d2 321 pc.baud(115200);
RiP 22:f38a15e851d2 322
RiP 21:3aecd735319d 323 //de biquad chains instellen
RiP 21:3aecd735319d 324 bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 );
RiP 21:3aecd735319d 325 bqc13.add( &bq131);
RiP 21:3aecd735319d 326 bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 );
RiP 21:3aecd735319d 327 bqc23.add( &bq231);
RiP 21:3aecd735319d 328 bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 );
RiP 21:3aecd735319d 329 bqc33.add( &bq331);
RiP 21:3aecd735319d 330 /*Attach the 'sample' function to the timer 'sample_timer'.
RiP 21:3aecd735319d 331 this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
vsluiter 4:8b298dfada81 332 */
RiP 27:1ff7fa636f1c 333 pos_timer.attach(&my_pos, 1);
RiP 23:54d28f9eef53 334 sample_timer.attach(&sampleflag, samplefreq);
RiP 24:01b4b51b5dc6 335 button_calibrate.fall(&buttonflag_go);
RiP 24:01b4b51b5dc6 336
tomlankhorst 15:0da764eea774 337
RiP 22:f38a15e851d2 338 while(1) {
RiP 22:f38a15e851d2 339 if (sampletimer==true) {
RiP 25:1a71424b05ff 340 //sample(mystate);
RiP 30:7cc564d38bc4 341 sample_button(mystate);
RiP 29:ac08c1a32c54 342 print_state();
RiP 29:ac08c1a32c54 343 change_ref();
RiP 29:ac08c1a32c54 344 sampletimer = false;
RiP 29:ac08c1a32c54 345
RiP 29:ac08c1a32c54 346
RiP 25:1a71424b05ff 347 }
RiP 22:f38a15e851d2 348 }
vsluiter 0:32bb76391d89 349 }