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@27:1ff7fa636f1c, 2016-10-21 (annotated)

Committer:
RiP
Date:
Fri Oct 21 13:32:25 2016 +0000
Revision:
27:1ff7fa636f1c
Parent:
26:91d48c0b722d
Child:
28:3b1b29193851
referentie signaal toegevoegd, maar hij verhoogt de referentie maar 1x

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