Jan Reinder / Mbed 2 deprecated EMG_filtering

10-25-16 filters in emg_filtering script

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of EMG_filtering by Rick Poppe

main.cpp@33:f8829d9e8192, 2016-10-25 (annotated)

Committer:
janreinder
Date:
Tue Oct 25 12:55:41 2016 +0000
Revision:
33:f8829d9e8192
Parent:
32:96d4a811ae32 
test values 2 (2:55 10-25-16)

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