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@31:9c3b022f1dc3, 2016-10-25 (annotated)

Committer:
RiP
Date:
Tue Oct 25 10:39:11 2016 +0000
Revision:
31:9c3b022f1dc3
Parent:
30:7cc564d38bc4
Child:
32:e1e5f03e9faf
rare shit

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 31:9c3b022f1dc3 27 double threshold = 0.11;
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 30:7cc564d38bc4 34 double speed=0.2;
RiP 31:9c3b022f1dc3 35 const int negative=-1;
RiP 31:9c3b022f1dc3 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 21:3aecd735319d 96 /* Read the emg signals and filter it*/
RiP 22:f38a15e851d2 97
RiP 21:3aecd735319d 98 scope.set(0, emg1.read()); //original signal
RiP 27:1ff7fa636f1c 99 emg02=bqc13.step(fabs(bqc11.step(emg1.read())));
RiP 21:3aecd735319d 100 scope.set(1, emg02);
RiP 21:3aecd735319d 101 /* Read the second emg signal and filter it*/
RiP 21:3aecd735319d 102 scope.set(2, emg2.read()); //original signal
RiP 27:1ff7fa636f1c 103 emg12=bqc23.step(fabs(bqc21.step(emg2.read())));
RiP 21:3aecd735319d 104 scope.set(3, emg12);
RiP 21:3aecd735319d 105 /* Read the third emg signal and filter it*/
RiP 21:3aecd735319d 106 scope.set(4, emg3.read()); //original signal
RiP 27:1ff7fa636f1c 107 emg22=bqc33.step(fabs(bqc31.step(emg3.read())));
RiP 21:3aecd735319d 108 scope.set(5, emg22);
RiP 29:ac08c1a32c54 109
RiP 22:f38a15e851d2 110 //pc.printf("Hello World!\n");
RiP 21:3aecd735319d 111 /* Ensure that enough channels are available (HIDScope scope( 2 ))
RiP 21:3aecd735319d 112 * Finally, send all channels to the PC at once */
vsluiter 11:ce72ec658a95 113 scope.send();
RiP 31:9c3b022f1dc3 114 if (pc.readable()){
RiP 31:9c3b022f1dc3 115 key=pc.getc();
RiP 31:9c3b022f1dc3 116 }
RiP 31:9c3b022f1dc3 117 else {
RiP 31:9c3b022f1dc3 118 key='p';
RiP 31:9c3b022f1dc3 119 }
RiP 25:1a71424b05ff 120
RiP 31:9c3b022f1dc3 121 if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
RiP 31:9c3b022f1dc3 122 mystate = STATE_XY_NEG;
RiP 31:9c3b022f1dc3 123 ref_x=ref_x+speed*negative;
RiP 31:9c3b022f1dc3 124 ref_y=ref_y+speed*negative;
RiP 31:9c3b022f1dc3 125
RiP 31:9c3b022f1dc3 126 } else if (emg02>threshold&&emg12>threshold || key=='a') {
RiP 31:9c3b022f1dc3 127 mystate = STATE_X_NEG;
RiP 31:9c3b022f1dc3 128 ref_x=ref_x+speed*negative;
RiP 23:54d28f9eef53 129
RiP 31:9c3b022f1dc3 130 } else if (emg02>threshold&&emg22>threshold || key=='s') {
RiP 31:9c3b022f1dc3 131 mystate = STATE_Y_NEG;
RiP 31:9c3b022f1dc3 132 ref_y=ref_y+speed*negative;
RiP 31:9c3b022f1dc3 133
RiP 31:9c3b022f1dc3 134 } else if (emg12>threshold&&emg22>threshold || key=='e') {
RiP 31:9c3b022f1dc3 135 mystate = STATE_XY;
RiP 31:9c3b022f1dc3 136 ref_x=ref_x+speed;
RiP 31:9c3b022f1dc3 137 ref_y=ref_y+speed;
RiP 31:9c3b022f1dc3 138
RiP 31:9c3b022f1dc3 139 } else if (emg12>threshold || key=='q') {
RiP 31:9c3b022f1dc3 140 mystate = STATE_X;
RiP 31:9c3b022f1dc3 141 ref_x=ref_x+speed;
RiP 31:9c3b022f1dc3 142
RiP 31:9c3b022f1dc3 143 } else if (emg22>threshold || key=='w') {
RiP 31:9c3b022f1dc3 144 mystate = STATE_Y;
RiP 31:9c3b022f1dc3 145 ref_y=ref_y+speed;
RiP 22:f38a15e851d2 146 } else {
RiP 31:9c3b022f1dc3 147 mystate = STATE_PAUZE;
RiP 22:f38a15e851d2 148 }
RiP 23:54d28f9eef53 149
RiP 24:01b4b51b5dc6 150 if (buttonflag==true) {
RiP 24:01b4b51b5dc6 151 mystate = STATE_CALIBRATION;
RiP 24:01b4b51b5dc6 152 }
RiP 24:01b4b51b5dc6 153
RiP 27:1ff7fa636f1c 154 if (myoldstate==mystate) {
RiP 27:1ff7fa636f1c 155 newcase=false;
RiP 31:9c3b022f1dc3 156
RiP 27:1ff7fa636f1c 157 } else {
RiP 27:1ff7fa636f1c 158 newcase=true;
RiP 27:1ff7fa636f1c 159 }
vsluiter 2:e314bb3b2d99 160 }
vsluiter 0:32bb76391d89 161
RiP 27:1ff7fa636f1c 162 void my_pos()
RiP 27:1ff7fa636f1c 163 {
RiP 27:1ff7fa636f1c 164 pc.printf("x_pos=%.4f\ty_pos=%.4f\n\r",ref_x,ref_y);
RiP 27:1ff7fa636f1c 165
RiP 27:1ff7fa636f1c 166 }
RiP 26:91d48c0b722d 167
RiP 29:ac08c1a32c54 168 void print_state()
RiP 28:3b1b29193851 169 {
RiP 31:9c3b022f1dc3 170 if (newcase==true) {
RiP 28:3b1b29193851 171 switch (mystate) {
RiP 28:3b1b29193851 172 case STATE_CALIBRATION : { // calibration
RiP 28:3b1b29193851 173 pc.printf("calibration\n\r");
RiP 28:3b1b29193851 174 break;
RiP 28:3b1b29193851 175 }
RiP 28:3b1b29193851 176 case STATE_X : // run
RiP 29:ac08c1a32c54 177 pc.printf("X\n\r");
RiP 28:3b1b29193851 178 break;
RiP 28:3b1b29193851 179 case STATE_X_NEG : // run
RiP 29:ac08c1a32c54 180 pc.printf("Xneg\n\r");
RiP 28:3b1b29193851 181 break;
RiP 28:3b1b29193851 182 case STATE_Y : // execute mode 1
RiP 29:ac08c1a32c54 183 pc.printf("Y\n\r");
RiP 28:3b1b29193851 184 break;
RiP 28:3b1b29193851 185 case STATE_Y_NEG : // execute mode 1
RiP 29:ac08c1a32c54 186 pc.printf("Yneg\n\r");
RiP 28:3b1b29193851 187 break;
RiP 28:3b1b29193851 188 case STATE_XY : // execute mode 2
RiP 29:ac08c1a32c54 189 pc.printf("XY\n\r");
RiP 28:3b1b29193851 190 break;
RiP 28:3b1b29193851 191 case STATE_XY_NEG : // execute mode 2
RiP 29:ac08c1a32c54 192 pc.printf("XYneg\n\r");
RiP 28:3b1b29193851 193 break;
RiP 28:3b1b29193851 194 case STATE_PAUZE : // default
RiP 29:ac08c1a32c54 195 pc.printf("PAUZE\n\r");
RiP 28:3b1b29193851 196 break;
RiP 28:3b1b29193851 197 }
RiP 28:3b1b29193851 198 }
RiP 28:3b1b29193851 199 }
RiP 28:3b1b29193851 200
vsluiter 0:32bb76391d89 201 int main()
RiP 21:3aecd735319d 202 {
RiP 29:ac08c1a32c54 203 pc.printf("RESET\n\r");
RiP 22:f38a15e851d2 204 pc.baud(115200);
RiP 22:f38a15e851d2 205
RiP 31:9c3b022f1dc3 206 //make the Biquad chains
RiP 21:3aecd735319d 207 bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 );
RiP 21:3aecd735319d 208 bqc13.add( &bq131);
RiP 21:3aecd735319d 209 bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 );
RiP 21:3aecd735319d 210 bqc23.add( &bq231);
RiP 21:3aecd735319d 211 bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 );
RiP 21:3aecd735319d 212 bqc33.add( &bq331);
RiP 21:3aecd735319d 213 /*Attach the 'sample' function to the timer 'sample_timer'.
RiP 21:3aecd735319d 214 this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
vsluiter 4:8b298dfada81 215 */
RiP 23:54d28f9eef53 216 sample_timer.attach(&sampleflag, samplefreq);
RiP 24:01b4b51b5dc6 217 button_calibrate.fall(&buttonflag_go);
RiP 31:9c3b022f1dc3 218 pos_timer.attach(&my_pos, 1);
tomlankhorst 15:0da764eea774 219
RiP 22:f38a15e851d2 220 while(1) {
RiP 22:f38a15e851d2 221 if (sampletimer==true) {
RiP 25:1a71424b05ff 222 //sample(mystate);
RiP 31:9c3b022f1dc3 223 sample(mystate);
RiP 29:ac08c1a32c54 224 print_state();
RiP 29:ac08c1a32c54 225 sampletimer = false;
RiP 25:1a71424b05ff 226 }
RiP 22:f38a15e851d2 227 }
vsluiter 0:32bb76391d89 228 }