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@37:af85a7b57a25, 2016-10-26 (annotated)

Committer:
RiP
Date:
Wed Oct 26 12:20:24 2016 +0000
Revision:
37:af85a7b57a25
Parent:
36:344588e69589 
limietwaarden toegevoegd, werkt alleen nog niet perfect

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
RiP 36:344588e69589 6
vsluiter 4:8b298dfada81 7 //Define objects
RiP 31:9c3b022f1dc3 8 //Define the button interrupt for the calibration
RiP 31:9c3b022f1dc3 9 InterruptIn button_calibrate(PTA4);
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 37:af85a7b57a25 21 MODSERIAL pc(USBTX, USBRX);
vsluiter 2:e314bb3b2d99 22
RiP 36:344588e69589 23 //Initialize all variables
RiP 22:f38a15e851d2 24 volatile bool sampletimer = false;
RiP 24:01b4b51b5dc6 25 volatile bool buttonflag = false;
RiP 25:1a71424b05ff 26 volatile bool newcase = false;
RiP 25:1a71424b05ff 27
RiP 33:fcd4568f1c86 28 double threshold = 0.04;
RiP 23:54d28f9eef53 29 double samplefreq=0.002;
RiP 25:1a71424b05ff 30 double emg02;
RiP 25:1a71424b05ff 31 double emg12;
RiP 25:1a71424b05ff 32 double emg22;
RiP 37:af85a7b57a25 33 double ref_x=0.0000;
RiP 37:af85a7b57a25 34 double ref_y=0.0000;
RiP 37:af85a7b57a25 35 double old_ref_x;
RiP 37:af85a7b57a25 36 double old_ref_y;
RiP 36:344588e69589 37 double speed_emg=0.00002;
RiP 36:344588e69589 38 double speed_key=0.000326;
RiP 37:af85a7b57a25 39 double speed=0.00002;
RiP 37:af85a7b57a25 40 double theta=0.0;
RiP 37:af85a7b57a25 41 double radius=0.0;
RiP 36:344588e69589 42
RiP 37:af85a7b57a25 43 const double minRadius=0.3; // minimum radius of arm
RiP 37:af85a7b57a25 44 const double maxRadius=0.6; // maximum radius of arm
RiP 37:af85a7b57a25 45 const double minAngle=-1.25; // minimum angle for limiting controller
RiP 37:af85a7b57a25 46
RiP 33:fcd4568f1c86 47 char key;
RiP 31:9c3b022f1dc3 48
RiP 27:1ff7fa636f1c 49
RiP 36:344588e69589 50 // create a variable called 'mystate', define it
RiP 23:54d28f9eef53 51 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 52 states mystate = STATE_PAUZE;
RiP 28:3b1b29193851 53
RiP 31:9c3b022f1dc3 54 //Define the needed Biquad chains
RiP 21:3aecd735319d 55 BiQuadChain bqc11;
RiP 21:3aecd735319d 56 BiQuadChain bqc13;
RiP 21:3aecd735319d 57 BiQuadChain bqc21;
RiP 21:3aecd735319d 58 BiQuadChain bqc23;
RiP 21:3aecd735319d 59 BiQuadChain bqc31;
RiP 21:3aecd735319d 60 BiQuadChain bqc33;
RiP 31:9c3b022f1dc3 61
RiP 31:9c3b022f1dc3 62 //Define the BiQuads for the filter of the first emg signal
RiP 31:9c3b022f1dc3 63 //Notch filter
RiP 21:3aecd735319d 64 BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
RiP 21:3aecd735319d 65 BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
RiP 21:3aecd735319d 66 BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
RiP 31:9c3b022f1dc3 67 //High pass filter
RiP 36:344588e69589 68 //BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Old biquad values
RiP 36:344588e69589 69 BiQuad bq121( 0.8956, -1.7911, 0.8956, 1.0000, -1.7814, 0.7941);
RiP 36:344588e69589 70 BiQuad bq122( 0.9192, -1.8385, 0.9192, 1.0000, -1.8319, 0.8450);
RiP 36:344588e69589 71 BiQuad bq123( 0.9649, -1.9298, 0.9649, 1.0000, -1.9266, 0.9403);
RiP 31:9c3b022f1dc3 72 //Low pass filter
RiP 21:3aecd735319d 73 BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
RiP 21:3aecd735319d 74
RiP 31:9c3b022f1dc3 75 //Define the BiQuads for the filter of the second emg signal
RiP 31:9c3b022f1dc3 76 //Notch filter
RiP 31:9c3b022f1dc3 77 BiQuad bq211 = bq111;
RiP 31:9c3b022f1dc3 78 BiQuad bq212 = bq112;
RiP 31:9c3b022f1dc3 79 BiQuad bq213 = bq113;
RiP 21:3aecd735319d 80 /* High pass filter*/
RiP 31:9c3b022f1dc3 81 BiQuad bq221 = bq121;
RiP 36:344588e69589 82 BiQuad bq222 = bq122;
RiP 36:344588e69589 83 BiQuad bq223 = bq123;
RiP 31:9c3b022f1dc3 84 /* Low pass filter*/
RiP 31:9c3b022f1dc3 85 BiQuad bq231 = bq131;
RiP 21:3aecd735319d 86
RiP 31:9c3b022f1dc3 87 //Define the BiQuads for the filter of the third emg signal
RiP 31:9c3b022f1dc3 88 //notch filter
RiP 31:9c3b022f1dc3 89 BiQuad bq311 = bq111;
RiP 31:9c3b022f1dc3 90 BiQuad bq312 = bq112;
RiP 31:9c3b022f1dc3 91 BiQuad bq313 = bq113;
RiP 31:9c3b022f1dc3 92 //High pass filter
RiP 31:9c3b022f1dc3 93 BiQuad bq321 = bq121;
RiP 36:344588e69589 94 BiQuad bq323 = bq122;
RiP 36:344588e69589 95 BiQuad bq322 = bq123;
RiP 31:9c3b022f1dc3 96 //low pass filter
RiP 31:9c3b022f1dc3 97 BiQuad bq331 = bq131;
RiP 22:f38a15e851d2 98
RiP 22:f38a15e851d2 99 void sampleflag()
RiP 22:f38a15e851d2 100 {
RiP 37:af85a7b57a25 101 if (sampletimer==true) {
RiP 37:af85a7b57a25 102 pc.printf("rate too high error in setgoflag\n\r");
RiP 37:af85a7b57a25 103 }
RiP 36:344588e69589 104 //This sets the go flag for when the function sample needs to be called
RiP 22:f38a15e851d2 105 sampletimer=true;
RiP 22:f38a15e851d2 106 }
RiP 22:f38a15e851d2 107
RiP 36:344588e69589 108 void calibrate()
RiP 24:01b4b51b5dc6 109 {
RiP 36:344588e69589 110 //This resets the reference signals so that the robot can be calibrated
RiP 37:af85a7b57a25 111 ref_x=0.0000;
RiP 37:af85a7b57a25 112 ref_y=0.0000;
RiP 24:01b4b51b5dc6 113 }
RiP 22:f38a15e851d2 114
RiP 23:54d28f9eef53 115 void sample(states &mystate)
vsluiter 2:e314bb3b2d99 116 {
RiP 27:1ff7fa636f1c 117 states myoldstate=mystate;
RiP 36:344588e69589 118
RiP 36:344588e69589 119 //This checks if a key is pressed and adjusts the speed to the needed speed
RiP 33:fcd4568f1c86 120 if (pc.readable()==1) {
RiP 37:af85a7b57a25 121
RiP 33:fcd4568f1c86 122 key=pc.getc();
RiP 37:af85a7b57a25 123 //printf("%c\n\r",key);
RiP 37:af85a7b57a25 124 }
RiP 37:af85a7b57a25 125
RiP 33:fcd4568f1c86 126
RiP 36:344588e69589 127 //Read the emg signals and filter it
RiP 22:f38a15e851d2 128
RiP 36:344588e69589 129 scope.set(0, emg1.read()); //original signal 0
RiP 36:344588e69589 130 emg02=bqc13.step(fabs(bqc11.step(emg1.read()))); //filtered signal 0
RiP 21:3aecd735319d 131 scope.set(1, emg02);
RiP 36:344588e69589 132 scope.set(2, emg2.read()); //original signal 1
RiP 36:344588e69589 133 emg12=bqc23.step(fabs(bqc21.step(emg2.read()))); //filtered signal 1
RiP 21:3aecd735319d 134 scope.set(3, emg12);
RiP 36:344588e69589 135 scope.set(4, emg3.read()); //original signal 2
RiP 36:344588e69589 136 emg22=bqc33.step(fabs(bqc31.step(emg3.read()))); //filtered signal 2
RiP 21:3aecd735319d 137 scope.set(5, emg22);
RiP 29:ac08c1a32c54 138
RiP 37:af85a7b57a25 139 emg02=1;
RiP 36:344588e69589 140 //Ensure that enough channels are available at the top (HIDScope scope( 6 ))
RiP 36:344588e69589 141 //Finally, send all channels to the PC at once
vsluiter 11:ce72ec658a95 142 scope.send();
RiP 25:1a71424b05ff 143
RiP 37:af85a7b57a25 144 old_ref_x=ref_x;
RiP 37:af85a7b57a25 145 old_ref_y=ref_y;
RiP 36:344588e69589 146 //look if the emg signals go over the threshold and change the state to the cooresponding state. Also change the reference.
RiP 33:fcd4568f1c86 147 if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
RiP 31:9c3b022f1dc3 148 mystate = STATE_XY_NEG;
RiP 37:af85a7b57a25 149 ref_x=ref_x-speed;
RiP 37:af85a7b57a25 150 ref_y=ref_y-speed;
RiP 31:9c3b022f1dc3 151
RiP 33:fcd4568f1c86 152 } else if (emg02>threshold&&emg12>threshold || key == 'a' ) {
RiP 31:9c3b022f1dc3 153 mystate = STATE_X_NEG;
RiP 37:af85a7b57a25 154 ref_x=ref_x-speed;
RiP 23:54d28f9eef53 155
RiP 33:fcd4568f1c86 156 } else if (emg02>threshold&&emg22>threshold || key == 's') {
RiP 31:9c3b022f1dc3 157 mystate = STATE_Y_NEG;
RiP 37:af85a7b57a25 158 ref_y=ref_y-speed;
RiP 31:9c3b022f1dc3 159
RiP 33:fcd4568f1c86 160 } else if (emg12>threshold&&emg22>threshold || key == 'e' ) {
RiP 31:9c3b022f1dc3 161 mystate = STATE_XY;
RiP 31:9c3b022f1dc3 162 ref_x=ref_x+speed;
RiP 31:9c3b022f1dc3 163 ref_y=ref_y+speed;
RiP 31:9c3b022f1dc3 164
RiP 33:fcd4568f1c86 165 } else if (emg12>threshold || key == 'q' ) {
RiP 31:9c3b022f1dc3 166 mystate = STATE_X;
RiP 31:9c3b022f1dc3 167 ref_x=ref_x+speed;
RiP 31:9c3b022f1dc3 168
RiP 33:fcd4568f1c86 169 } else if (emg22>threshold || key == 'w') {
RiP 31:9c3b022f1dc3 170 mystate = STATE_Y;
RiP 31:9c3b022f1dc3 171 ref_y=ref_y+speed;
RiP 22:f38a15e851d2 172 } else {
RiP 37:af85a7b57a25 173 mystate = STATE_PAUZE;
RiP 37:af85a7b57a25 174 }
RiP 37:af85a7b57a25 175
RiP 37:af85a7b57a25 176 // convert ref to gearbox angle
RiP 37:af85a7b57a25 177 theta=atan((ref_y+sin(theta)*minRadius)/(ref_x+cos(theta)*minRadius));
RiP 37:af85a7b57a25 178 radius=sqrt(pow(ref_x+cos(theta)*minRadius,2)+pow(ref_y+sin(theta)*minRadius,2));
RiP 37:af85a7b57a25 179 if (theta != theta) {
RiP 37:af85a7b57a25 180 theta=0;
RiP 37:af85a7b57a25 181 }
RiP 37:af85a7b57a25 182 if (theta <= minAngle) {
RiP 37:af85a7b57a25 183 ref_x=old_ref_x;
RiP 37:af85a7b57a25 184 ref_y=old_ref_y;
RiP 37:af85a7b57a25 185 pc.printf("fout 1 ");
RiP 37:af85a7b57a25 186 } else if (radius < minRadius) {
RiP 37:af85a7b57a25 187 ref_x=old_ref_x;
RiP 37:af85a7b57a25 188 ref_y=old_ref_y;
RiP 37:af85a7b57a25 189 pc.printf("fout 2 ");
RiP 37:af85a7b57a25 190 } /*else if (theta >= 0 ) {
RiP 37:af85a7b57a25 191 ref_x=old_ref_x;
RiP 37:af85a7b57a25 192 ref_y=old_ref_y;
RiP 37:af85a7b57a25 193 pc.printf("fout 3 ");
RiP 37:af85a7b57a25 194 }*/ else if ( radius > maxRadius) {
RiP 37:af85a7b57a25 195 ref_x=old_ref_x;
RiP 37:af85a7b57a25 196 ref_y=old_ref_y;
RiP 37:af85a7b57a25 197 pc.printf("fout 4 ");
RiP 22:f38a15e851d2 198 }
RiP 23:54d28f9eef53 199
RiP 36:344588e69589 200 //change newcase so that the state will only be printed once
RiP 27:1ff7fa636f1c 201 if (myoldstate==mystate) {
RiP 27:1ff7fa636f1c 202 newcase=false;
RiP 31:9c3b022f1dc3 203
RiP 27:1ff7fa636f1c 204 } else {
RiP 27:1ff7fa636f1c 205 newcase=true;
RiP 27:1ff7fa636f1c 206 }
vsluiter 2:e314bb3b2d99 207 }
vsluiter 0:32bb76391d89 208
RiP 27:1ff7fa636f1c 209 void my_pos()
RiP 27:1ff7fa636f1c 210 {
RiP 36:344588e69589 211 //This function is attached to a ticker so that the reference position is printed every second.
RiP 37:af85a7b57a25 212 pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta);
RiP 27:1ff7fa636f1c 213
RiP 27:1ff7fa636f1c 214 }
RiP 26:91d48c0b722d 215
RiP 29:ac08c1a32c54 216 void print_state()
RiP 28:3b1b29193851 217 {
RiP 36:344588e69589 218 //This code looks in which state the robot is in and prints it to the screen
RiP 31:9c3b022f1dc3 219 if (newcase==true) {
RiP 28:3b1b29193851 220 switch (mystate) {
RiP 28:3b1b29193851 221 case STATE_CALIBRATION : { // calibration
RiP 28:3b1b29193851 222 pc.printf("calibration\n\r");
RiP 28:3b1b29193851 223 break;
RiP 28:3b1b29193851 224 }
RiP 36:344588e69589 225 case STATE_X : // X direction
RiP 29:ac08c1a32c54 226 pc.printf("X\n\r");
RiP 28:3b1b29193851 227 break;
RiP 36:344588e69589 228 case STATE_X_NEG : // negative X direction
RiP 29:ac08c1a32c54 229 pc.printf("Xneg\n\r");
RiP 28:3b1b29193851 230 break;
RiP 36:344588e69589 231 case STATE_Y : // Y direction
RiP 29:ac08c1a32c54 232 pc.printf("Y\n\r");
RiP 28:3b1b29193851 233 break;
RiP 36:344588e69589 234 case STATE_Y_NEG : // negative Y direction
RiP 29:ac08c1a32c54 235 pc.printf("Yneg\n\r");
RiP 28:3b1b29193851 236 break;
RiP 36:344588e69589 237 case STATE_XY : // X&Y direction
RiP 29:ac08c1a32c54 238 pc.printf("XY\n\r");
RiP 28:3b1b29193851 239 break;
RiP 36:344588e69589 240 case STATE_XY_NEG : // negative X&Y direction
RiP 29:ac08c1a32c54 241 pc.printf("XYneg\n\r");
RiP 28:3b1b29193851 242 break;
RiP 36:344588e69589 243 case STATE_PAUZE : // Pauze: do nothing
RiP 29:ac08c1a32c54 244 pc.printf("PAUZE\n\r");
RiP 28:3b1b29193851 245 break;
RiP 28:3b1b29193851 246 }
RiP 28:3b1b29193851 247 }
RiP 28:3b1b29193851 248 }
RiP 28:3b1b29193851 249
vsluiter 0:32bb76391d89 250 int main()
RiP 21:3aecd735319d 251 {
RiP 29:ac08c1a32c54 252 pc.printf("RESET\n\r");
RiP 22:f38a15e851d2 253 pc.baud(115200);
RiP 22:f38a15e851d2 254
RiP 36:344588e69589 255 //Initialize the Biquad chains
RiP 36:344588e69589 256 bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 );
RiP 21:3aecd735319d 257 bqc13.add( &bq131);
RiP 36:344588e69589 258 bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
RiP 21:3aecd735319d 259 bqc23.add( &bq231);
RiP 36:344588e69589 260 bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
RiP 21:3aecd735319d 261 bqc33.add( &bq331);
RiP 36:344588e69589 262
RiP 36:344588e69589 263 //Attach the 'sample' function to the timer 'sample_timer'.
RiP 36:344588e69589 264 //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
RiP 23:54d28f9eef53 265 sample_timer.attach(&sampleflag, samplefreq);
RiP 36:344588e69589 266 //Attach the function calibrate to the button interrupt
RiP 36:344588e69589 267 button_calibrate.fall(&calibrate);
RiP 36:344588e69589 268 //Attach the function my_pos to the timer pos_timer.
RiP 36:344588e69589 269 //This ensures that the position is printed every second.
RiP 31:9c3b022f1dc3 270 pos_timer.attach(&my_pos, 1);
tomlankhorst 15:0da764eea774 271
RiP 22:f38a15e851d2 272 while(1) {
RiP 36:344588e69589 273 //Only take samples when the go flag is true.
RiP 22:f38a15e851d2 274 if (sampletimer==true) {
RiP 31:9c3b022f1dc3 275 sample(mystate);
RiP 36:344588e69589 276 print_state();
RiP 29:ac08c1a32c54 277 sampletimer = false;
RiP 25:1a71424b05ff 278 }
RiP 22:f38a15e851d2 279 }
vsluiter 0:32bb76391d89 280 }