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@25:1a71424b05ff, 2016-10-20 (annotated)

Committer:
RiP
Date:
Thu Oct 20 10:31:45 2016 +0000
Revision:
25:1a71424b05ff
Parent:
24:01b4b51b5dc6
Child:
26:91d48c0b722d
buttons in plaats van emg, maar calibration interrupt werkt niet meer

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