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@33:fcd4568f1c86, 2016-10-25 (annotated)

Committer:
RiP
Date:
Tue Oct 25 12:50:55 2016 +0000
Revision:
33:fcd4568f1c86
Parent:
32:e1e5f03e9faf
Child:
35:4905144c1123
werkend met 1 cm per seconde

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