the emg filtering part of the program
Dependencies: HIDScope biquadFilter mbed MODSERIAL
Fork of EMG by
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?
User | Revision | Line number | New 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 | } |