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@35:4905144c1123, 2016-10-26 (annotated)

Committer:
RiP
Date:
Wed Oct 26 08:52:05 2016 +0000
Revision:
35:4905144c1123
Parent:
33:fcd4568f1c86
Child:
36:344588e69589
stripped version with hidscope

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 31:9c3b022f1dc3 7 //Define the button interrupt for the calibration
RiP 31:9c3b022f1dc3 8 InterruptIn button_calibrate(PTA4);
RiP 24:01b4b51b5dc6 9
RiP 31:9c3b022f1dc3 10 //Define the EMG inputs
RiP 21:3aecd735319d 11 AnalogIn emg1( A0 );
RiP 21:3aecd735319d 12 AnalogIn emg2( A1 );
RiP 21:3aecd735319d 13 AnalogIn emg3( A2 );
tomlankhorst 19:2bf824669684 14
RiP 31:9c3b022f1dc3 15 //Define the Tickers
RiP 27:1ff7fa636f1c 16 Ticker pos_timer;
tomlankhorst 14:f83354387756 17 Ticker sample_timer;
RiP 31:9c3b022f1dc3 18
RiP 21:3aecd735319d 19 HIDScope scope( 6 );
RiP 22:f38a15e851d2 20 MODSERIAL pc(USBTX, USBRX);
vsluiter 2:e314bb3b2d99 21
RiP 22:f38a15e851d2 22 volatile bool sampletimer = false;
RiP 24:01b4b51b5dc6 23 volatile bool buttonflag = false;
RiP 25:1a71424b05ff 24 volatile bool newcase = false;
RiP 25:1a71424b05ff 25
RiP 33:fcd4568f1c86 26 double threshold = 0.04;
RiP 23:54d28f9eef53 27 double samplefreq=0.002;
RiP 25:1a71424b05ff 28 double emg02;
RiP 25:1a71424b05ff 29 double emg12;
RiP 25:1a71424b05ff 30 double emg22;
RiP 27:1ff7fa636f1c 31 double ref_x=0.000;
RiP 27:1ff7fa636f1c 32 double ref_y=0.000;
RiP 33:fcd4568f1c86 33 double speed=0.00002;
RiP 31:9c3b022f1dc3 34 const int negative=-1;
RiP 33:fcd4568f1c86 35 char key;
RiP 31:9c3b022f1dc3 36
RiP 27:1ff7fa636f1c 37
RiP 28:3b1b29193851 38 // create a variable called 'state', define it
RiP 23:54d28f9eef53 39 typedef enum { STATE_CALIBRATION, STATE_PAUZE, STATE_X, STATE_X_NEG, STATE_Y, STATE_Y_NEG, STATE_XY, STATE_XY_NEG } states;
RiP 28:3b1b29193851 40 states mystate = STATE_PAUZE;
RiP 28:3b1b29193851 41
RiP 31:9c3b022f1dc3 42 //Define the needed Biquad chains
RiP 21:3aecd735319d 43 BiQuadChain bqc11;
RiP 21:3aecd735319d 44 BiQuadChain bqc13;
RiP 21:3aecd735319d 45 BiQuadChain bqc21;
RiP 21:3aecd735319d 46 BiQuadChain bqc23;
RiP 21:3aecd735319d 47 BiQuadChain bqc31;
RiP 21:3aecd735319d 48 BiQuadChain bqc33;
RiP 31:9c3b022f1dc3 49
RiP 31:9c3b022f1dc3 50 //Define the BiQuads for the filter of the first emg signal
RiP 31:9c3b022f1dc3 51 //Notch filter
RiP 21:3aecd735319d 52 BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
RiP 21:3aecd735319d 53 BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
RiP 21:3aecd735319d 54 BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
RiP 31:9c3b022f1dc3 55 //High pass filter
RiP 21:3aecd735319d 56 BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 );
RiP 31:9c3b022f1dc3 57 //Low pass filter
RiP 21:3aecd735319d 58 BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
RiP 21:3aecd735319d 59
RiP 31:9c3b022f1dc3 60 //Define the BiQuads for the filter of the second emg signal
RiP 31:9c3b022f1dc3 61 //Notch filter
RiP 31:9c3b022f1dc3 62 BiQuad bq211 = bq111;
RiP 31:9c3b022f1dc3 63 BiQuad bq212 = bq112;
RiP 31:9c3b022f1dc3 64 BiQuad bq213 = bq113;
RiP 21:3aecd735319d 65 /* High pass filter*/
RiP 31:9c3b022f1dc3 66 BiQuad bq221 = bq121;
RiP 31:9c3b022f1dc3 67 /* Low pass filter*/
RiP 31:9c3b022f1dc3 68 BiQuad bq231 = bq131;
RiP 21:3aecd735319d 69
RiP 31:9c3b022f1dc3 70 //Define the BiQuads for the filter of the third emg signal
RiP 31:9c3b022f1dc3 71 //notch filter
RiP 31:9c3b022f1dc3 72 BiQuad bq311 = bq111;
RiP 31:9c3b022f1dc3 73 BiQuad bq312 = bq112;
RiP 31:9c3b022f1dc3 74 BiQuad bq313 = bq113;
RiP 31:9c3b022f1dc3 75 //High pass filter
RiP 31:9c3b022f1dc3 76 BiQuad bq321 = bq121;
RiP 31:9c3b022f1dc3 77 //low pass filter
RiP 31:9c3b022f1dc3 78 BiQuad bq331 = bq131;
RiP 22:f38a15e851d2 79
RiP 27:1ff7fa636f1c 80
RiP 22:f38a15e851d2 81 void sampleflag()
RiP 22:f38a15e851d2 82 {
RiP 22:f38a15e851d2 83 sampletimer=true;
RiP 22:f38a15e851d2 84 }
RiP 22:f38a15e851d2 85
RiP 24:01b4b51b5dc6 86 void buttonflag_go()
RiP 24:01b4b51b5dc6 87 {
RiP 24:01b4b51b5dc6 88 buttonflag=true;
RiP 24:01b4b51b5dc6 89 }
RiP 22:f38a15e851d2 90
RiP 23:54d28f9eef53 91 void sample(states &mystate)
vsluiter 2:e314bb3b2d99 92 {
RiP 27:1ff7fa636f1c 93 states myoldstate=mystate;
RiP 35:4905144c1123 94
RiP 33:fcd4568f1c86 95 if (pc.readable()==1) {
RiP 33:fcd4568f1c86 96 key=pc.getc();
RiP 33:fcd4568f1c86 97 speed=0.000326;
RiP 33:fcd4568f1c86 98 } else {
RiP 33:fcd4568f1c86 99 key ='p';
RiP 33:fcd4568f1c86 100 speed=0.00002;
RiP 33:fcd4568f1c86 101 }
RiP 33:fcd4568f1c86 102
RiP 21:3aecd735319d 103 /* Read the emg signals and filter it*/
RiP 22:f38a15e851d2 104
RiP 21:3aecd735319d 105 scope.set(0, emg1.read()); //original signal
RiP 27:1ff7fa636f1c 106 emg02=bqc13.step(fabs(bqc11.step(emg1.read())));
RiP 21:3aecd735319d 107 scope.set(1, emg02);
RiP 21:3aecd735319d 108 /* Read the second emg signal and filter it*/
RiP 21:3aecd735319d 109 scope.set(2, emg2.read()); //original signal
RiP 27:1ff7fa636f1c 110 emg12=bqc23.step(fabs(bqc21.step(emg2.read())));
RiP 21:3aecd735319d 111 scope.set(3, emg12);
RiP 21:3aecd735319d 112 /* Read the third emg signal and filter it*/
RiP 21:3aecd735319d 113 scope.set(4, emg3.read()); //original signal
RiP 27:1ff7fa636f1c 114 emg22=bqc33.step(fabs(bqc31.step(emg3.read())));
RiP 21:3aecd735319d 115 scope.set(5, emg22);
RiP 29:ac08c1a32c54 116
RiP 21:3aecd735319d 117 /* Ensure that enough channels are available (HIDScope scope( 2 ))
RiP 21:3aecd735319d 118 * Finally, send all channels to the PC at once */
vsluiter 11:ce72ec658a95 119 scope.send();
RiP 25:1a71424b05ff 120
RiP 33:fcd4568f1c86 121
RiP 33:fcd4568f1c86 122 if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
RiP 31:9c3b022f1dc3 123 mystate = STATE_XY_NEG;
RiP 31:9c3b022f1dc3 124 ref_x=ref_x+speed*negative;
RiP 31:9c3b022f1dc3 125 ref_y=ref_y+speed*negative;
RiP 31:9c3b022f1dc3 126
RiP 33:fcd4568f1c86 127 } else if (emg02>threshold&&emg12>threshold || key == 'a' ) {
RiP 31:9c3b022f1dc3 128 mystate = STATE_X_NEG;
RiP 31:9c3b022f1dc3 129 ref_x=ref_x+speed*negative;
RiP 23:54d28f9eef53 130
RiP 33:fcd4568f1c86 131 } else if (emg02>threshold&&emg22>threshold || key == 's') {
RiP 31:9c3b022f1dc3 132 mystate = STATE_Y_NEG;
RiP 31:9c3b022f1dc3 133 ref_y=ref_y+speed*negative;
RiP 31:9c3b022f1dc3 134
RiP 33:fcd4568f1c86 135 } else if (emg12>threshold&&emg22>threshold || key == 'e' ) {
RiP 31:9c3b022f1dc3 136 mystate = STATE_XY;
RiP 31:9c3b022f1dc3 137 ref_x=ref_x+speed;
RiP 31:9c3b022f1dc3 138 ref_y=ref_y+speed;
RiP 31:9c3b022f1dc3 139
RiP 33:fcd4568f1c86 140 } else if (emg12>threshold || key == 'q' ) {
RiP 31:9c3b022f1dc3 141 mystate = STATE_X;
RiP 31:9c3b022f1dc3 142 ref_x=ref_x+speed;
RiP 31:9c3b022f1dc3 143
RiP 33:fcd4568f1c86 144 } else if (emg22>threshold || key == 'w') {
RiP 31:9c3b022f1dc3 145 mystate = STATE_Y;
RiP 31:9c3b022f1dc3 146 ref_y=ref_y+speed;
RiP 22:f38a15e851d2 147 } else {
RiP 31:9c3b022f1dc3 148 mystate = STATE_PAUZE;
RiP 22:f38a15e851d2 149 }
RiP 23:54d28f9eef53 150
RiP 24:01b4b51b5dc6 151 if (buttonflag==true) {
RiP 24:01b4b51b5dc6 152 mystate = STATE_CALIBRATION;
RiP 24:01b4b51b5dc6 153 }
RiP 24:01b4b51b5dc6 154
RiP 27:1ff7fa636f1c 155 if (myoldstate==mystate) {
RiP 27:1ff7fa636f1c 156 newcase=false;
RiP 31:9c3b022f1dc3 157
RiP 27:1ff7fa636f1c 158 } else {
RiP 27:1ff7fa636f1c 159 newcase=true;
RiP 27:1ff7fa636f1c 160 }
vsluiter 2:e314bb3b2d99 161 }
vsluiter 0:32bb76391d89 162
RiP 27:1ff7fa636f1c 163 void my_pos()
RiP 27:1ff7fa636f1c 164 {
RiP 27:1ff7fa636f1c 165 pc.printf("x_pos=%.4f\ty_pos=%.4f\n\r",ref_x,ref_y);
RiP 27:1ff7fa636f1c 166
RiP 27:1ff7fa636f1c 167 }
RiP 26:91d48c0b722d 168
RiP 29:ac08c1a32c54 169 void print_state()
RiP 28:3b1b29193851 170 {
RiP 31:9c3b022f1dc3 171 if (newcase==true) {
RiP 28:3b1b29193851 172 switch (mystate) {
RiP 28:3b1b29193851 173 case STATE_CALIBRATION : { // calibration
RiP 28:3b1b29193851 174 pc.printf("calibration\n\r");
RiP 28:3b1b29193851 175 break;
RiP 28:3b1b29193851 176 }
RiP 28:3b1b29193851 177 case STATE_X : // run
RiP 29:ac08c1a32c54 178 pc.printf("X\n\r");
RiP 28:3b1b29193851 179 break;
RiP 28:3b1b29193851 180 case STATE_X_NEG : // run
RiP 29:ac08c1a32c54 181 pc.printf("Xneg\n\r");
RiP 28:3b1b29193851 182 break;
RiP 28:3b1b29193851 183 case STATE_Y : // execute mode 1
RiP 29:ac08c1a32c54 184 pc.printf("Y\n\r");
RiP 28:3b1b29193851 185 break;
RiP 28:3b1b29193851 186 case STATE_Y_NEG : // execute mode 1
RiP 29:ac08c1a32c54 187 pc.printf("Yneg\n\r");
RiP 28:3b1b29193851 188 break;
RiP 28:3b1b29193851 189 case STATE_XY : // execute mode 2
RiP 29:ac08c1a32c54 190 pc.printf("XY\n\r");
RiP 28:3b1b29193851 191 break;
RiP 28:3b1b29193851 192 case STATE_XY_NEG : // execute mode 2
RiP 29:ac08c1a32c54 193 pc.printf("XYneg\n\r");
RiP 28:3b1b29193851 194 break;
RiP 28:3b1b29193851 195 case STATE_PAUZE : // default
RiP 29:ac08c1a32c54 196 pc.printf("PAUZE\n\r");
RiP 28:3b1b29193851 197 break;
RiP 28:3b1b29193851 198 }
RiP 28:3b1b29193851 199 }
RiP 28:3b1b29193851 200 }
RiP 28:3b1b29193851 201
vsluiter 0:32bb76391d89 202 int main()
RiP 21:3aecd735319d 203 {
RiP 29:ac08c1a32c54 204 pc.printf("RESET\n\r");
RiP 22:f38a15e851d2 205 pc.baud(115200);
RiP 22:f38a15e851d2 206
RiP 31:9c3b022f1dc3 207 //make the Biquad chains
RiP 21:3aecd735319d 208 bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 );
RiP 21:3aecd735319d 209 bqc13.add( &bq131);
RiP 21:3aecd735319d 210 bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 );
RiP 21:3aecd735319d 211 bqc23.add( &bq231);
RiP 21:3aecd735319d 212 bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 );
RiP 21:3aecd735319d 213 bqc33.add( &bq331);
RiP 21:3aecd735319d 214 /*Attach the 'sample' function to the timer 'sample_timer'.
RiP 21:3aecd735319d 215 this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
vsluiter 4:8b298dfada81 216 */
RiP 23:54d28f9eef53 217 sample_timer.attach(&sampleflag, samplefreq);
RiP 24:01b4b51b5dc6 218 button_calibrate.fall(&buttonflag_go);
RiP 31:9c3b022f1dc3 219 pos_timer.attach(&my_pos, 1);
tomlankhorst 15:0da764eea774 220
RiP 22:f38a15e851d2 221 while(1) {
RiP 22:f38a15e851d2 222 if (sampletimer==true) {
RiP 25:1a71424b05ff 223 //sample(mystate);
RiP 31:9c3b022f1dc3 224 sample(mystate);
RiP 33:fcd4568f1c86 225 // print_state();
RiP 29:ac08c1a32c54 226 sampletimer = false;
RiP 25:1a71424b05ff 227 }
RiP 22:f38a15e851d2 228 }
vsluiter 0:32bb76391d89 229 }