the emg filtering part of the program
Dependencies: HIDScope biquadFilter mbed MODSERIAL
Fork of EMG by
main.cpp@30:7cc564d38bc4, 2016-10-25 (annotated)
- Committer:
- RiP
- Date:
- Tue Oct 25 08:04:01 2016 +0000
- Revision:
- 30:7cc564d38bc4
- Parent:
- 29:ac08c1a32c54
- Child:
- 31:9c3b022f1dc3
knopjes werken nu ook
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 | |
vsluiter | 4:8b298dfada81 | 6 | //Define objects |
RiP | 24:01b4b51b5dc6 | 7 | |
RiP | 24:01b4b51b5dc6 | 8 | InterruptIn button_calibrate(PTA4); |
RiP | 21:3aecd735319d | 9 | AnalogIn emg1( A0 ); |
RiP | 21:3aecd735319d | 10 | AnalogIn emg2( A1 ); |
RiP | 21:3aecd735319d | 11 | AnalogIn emg3( A2 ); |
tomlankhorst | 19:2bf824669684 | 12 | |
RiP | 27:1ff7fa636f1c | 13 | Ticker pos_timer; |
tomlankhorst | 14:f83354387756 | 14 | Ticker sample_timer; |
RiP | 21:3aecd735319d | 15 | HIDScope scope( 6 ); |
RiP | 22:f38a15e851d2 | 16 | MODSERIAL pc(USBTX, USBRX); |
tomlankhorst | 18:21d8e7a81cf5 | 17 | DigitalOut led(LED1); |
vsluiter | 2:e314bb3b2d99 | 18 | |
RiP | 22:f38a15e851d2 | 19 | volatile bool sampletimer = false; |
RiP | 24:01b4b51b5dc6 | 20 | volatile bool buttonflag = false; |
RiP | 25:1a71424b05ff | 21 | volatile bool newcase = false; |
RiP | 28:3b1b29193851 | 22 | volatile bool newcaseprint = false; |
RiP | 29:ac08c1a32c54 | 23 | volatile bool change_ref_timer = false; |
RiP | 25:1a71424b05ff | 24 | |
RiP | 23:54d28f9eef53 | 25 | double threshold = 0.04; |
RiP | 23:54d28f9eef53 | 26 | double samplefreq=0.002; |
RiP | 25:1a71424b05ff | 27 | double emg02; |
RiP | 25:1a71424b05ff | 28 | double emg12; |
RiP | 25:1a71424b05ff | 29 | double emg22; |
RiP | 27:1ff7fa636f1c | 30 | double ref_x=0.000; |
RiP | 27:1ff7fa636f1c | 31 | double ref_y=0.000; |
RiP | 30:7cc564d38bc4 | 32 | double speed=0.2; |
RiP | 27:1ff7fa636f1c | 33 | |
RiP | 28:3b1b29193851 | 34 | // create a variable called 'state', define it |
RiP | 23:54d28f9eef53 | 35 | typedef enum { STATE_CALIBRATION, STATE_PAUZE, STATE_X, STATE_X_NEG, STATE_Y, STATE_Y_NEG, STATE_XY, STATE_XY_NEG } states; |
RiP | 22:f38a15e851d2 | 36 | |
RiP | 28:3b1b29193851 | 37 | states mystate = STATE_PAUZE; |
RiP | 28:3b1b29193851 | 38 | |
RiP | 21:3aecd735319d | 39 | BiQuadChain bqc11; |
RiP | 21:3aecd735319d | 40 | BiQuadChain bqc13; |
RiP | 21:3aecd735319d | 41 | BiQuadChain bqc21; |
RiP | 21:3aecd735319d | 42 | BiQuadChain bqc23; |
RiP | 21:3aecd735319d | 43 | BiQuadChain bqc31; |
RiP | 21:3aecd735319d | 44 | BiQuadChain bqc33; |
RiP | 21:3aecd735319d | 45 | //BiQuad bq11( 9.87589e-01, -1.59795e+00, 9.87589e-01, -1.59795e+00, 9.75178e-01 ); //oude BiQuad waardes |
RiP | 21:3aecd735319d | 46 | /* BiQuads for filter emg1 |
RiP | 21:3aecd735319d | 47 | notch filter*/ |
RiP | 21:3aecd735319d | 48 | BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589); |
RiP | 21:3aecd735319d | 49 | BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787); |
RiP | 21:3aecd735319d | 50 | BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798); |
RiP | 21:3aecd735319d | 51 | /* High pass filter*/ |
RiP | 21:3aecd735319d | 52 | BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); |
RiP | 21:3aecd735319d | 53 | /* low pass filter*/ |
RiP | 21:3aecd735319d | 54 | BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 ); |
RiP | 21:3aecd735319d | 55 | |
RiP | 21:3aecd735319d | 56 | /* BiQuads for filter emg2 |
RiP | 21:3aecd735319d | 57 | notch filter*/ |
RiP | 21:3aecd735319d | 58 | BiQuad bq211(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589); |
RiP | 21:3aecd735319d | 59 | BiQuad bq212(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787); |
RiP | 21:3aecd735319d | 60 | BiQuad bq213(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798); |
RiP | 21:3aecd735319d | 61 | /* High pass filter*/ |
RiP | 21:3aecd735319d | 62 | BiQuad bq221( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); |
RiP | 21:3aecd735319d | 63 | /* low pass filter*/ |
RiP | 21:3aecd735319d | 64 | BiQuad bq231( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 ); |
RiP | 21:3aecd735319d | 65 | |
RiP | 21:3aecd735319d | 66 | /* BiQuads for filter emg3 |
RiP | 21:3aecd735319d | 67 | notch filter*/ |
RiP | 21:3aecd735319d | 68 | BiQuad bq311(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589); |
RiP | 21:3aecd735319d | 69 | BiQuad bq312(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787); |
RiP | 21:3aecd735319d | 70 | BiQuad bq313(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798); |
RiP | 21:3aecd735319d | 71 | /* High pass filter*/ |
RiP | 21:3aecd735319d | 72 | BiQuad bq321( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); |
RiP | 21:3aecd735319d | 73 | /* low pass filter*/ |
RiP | 21:3aecd735319d | 74 | BiQuad bq331( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 ); |
RiP | 21:3aecd735319d | 75 | |
RiP | 22:f38a15e851d2 | 76 | |
RiP | 27:1ff7fa636f1c | 77 | |
RiP | 22:f38a15e851d2 | 78 | void sampleflag() |
RiP | 22:f38a15e851d2 | 79 | { |
RiP | 22:f38a15e851d2 | 80 | sampletimer=true; |
RiP | 29:ac08c1a32c54 | 81 | change_ref_timer=true; |
RiP | 22:f38a15e851d2 | 82 | } |
RiP | 22:f38a15e851d2 | 83 | |
RiP | 24:01b4b51b5dc6 | 84 | void buttonflag_go() |
RiP | 24:01b4b51b5dc6 | 85 | { |
RiP | 24:01b4b51b5dc6 | 86 | buttonflag=true; |
RiP | 24:01b4b51b5dc6 | 87 | } |
RiP | 22:f38a15e851d2 | 88 | |
RiP | 25:1a71424b05ff | 89 | void sample_button(states &mystate) |
RiP | 25:1a71424b05ff | 90 | { |
RiP | 25:1a71424b05ff | 91 | states myoldstate=mystate; |
RiP | 25:1a71424b05ff | 92 | char key=pc.getc(); |
RiP | 25:1a71424b05ff | 93 | |
RiP | 26:91d48c0b722d | 94 | // pc.printf("%c/n",key); |
RiP | 25:1a71424b05ff | 95 | |
RiP | 25:1a71424b05ff | 96 | switch (key) { |
RiP | 25:1a71424b05ff | 97 | case 'p' : // run |
RiP | 25:1a71424b05ff | 98 | emg02=0.0; |
RiP | 25:1a71424b05ff | 99 | emg12=0.0; |
RiP | 25:1a71424b05ff | 100 | emg22=0.0; |
RiP | 25:1a71424b05ff | 101 | break; |
RiP | 25:1a71424b05ff | 102 | case 'q' : // run |
RiP | 25:1a71424b05ff | 103 | emg02=0.0; |
RiP | 25:1a71424b05ff | 104 | emg12=1.0; |
RiP | 25:1a71424b05ff | 105 | emg22=0.0; |
RiP | 25:1a71424b05ff | 106 | break; |
RiP | 25:1a71424b05ff | 107 | case 'w' : // run |
RiP | 25:1a71424b05ff | 108 | emg02=0.0; |
RiP | 25:1a71424b05ff | 109 | emg12=0.0; |
RiP | 25:1a71424b05ff | 110 | emg22=1.0; |
RiP | 25:1a71424b05ff | 111 | break; |
RiP | 25:1a71424b05ff | 112 | case 'e' : // run |
RiP | 25:1a71424b05ff | 113 | emg02=0.0; |
RiP | 25:1a71424b05ff | 114 | emg12=1.0; |
RiP | 25:1a71424b05ff | 115 | emg22=1.0; |
RiP | 25:1a71424b05ff | 116 | break; |
RiP | 25:1a71424b05ff | 117 | case 'a' : // run |
RiP | 25:1a71424b05ff | 118 | emg02=1.0; |
RiP | 25:1a71424b05ff | 119 | emg12=1.0; |
RiP | 25:1a71424b05ff | 120 | emg22=0.0; |
RiP | 25:1a71424b05ff | 121 | break; |
RiP | 25:1a71424b05ff | 122 | case 's' : // run |
RiP | 25:1a71424b05ff | 123 | emg02=1.0; |
RiP | 25:1a71424b05ff | 124 | emg12=0.0; |
RiP | 25:1a71424b05ff | 125 | emg22=1.0; |
RiP | 25:1a71424b05ff | 126 | break; |
RiP | 25:1a71424b05ff | 127 | case 'd' : // run |
RiP | 25:1a71424b05ff | 128 | emg02=1.0; |
RiP | 25:1a71424b05ff | 129 | emg12=1.0; |
RiP | 25:1a71424b05ff | 130 | emg22=1.0; |
RiP | 25:1a71424b05ff | 131 | break; |
RiP | 25:1a71424b05ff | 132 | } |
RiP | 25:1a71424b05ff | 133 | |
RiP | 25:1a71424b05ff | 134 | |
RiP | 25:1a71424b05ff | 135 | |
RiP | 25:1a71424b05ff | 136 | |
RiP | 25:1a71424b05ff | 137 | |
RiP | 25:1a71424b05ff | 138 | led = !led; |
RiP | 25:1a71424b05ff | 139 | if (emg02>threshold) { |
RiP | 25:1a71424b05ff | 140 | if (emg12>threshold&&emg22>threshold) { |
RiP | 25:1a71424b05ff | 141 | mystate = STATE_XY_NEG; |
RiP | 25:1a71424b05ff | 142 | } else if (emg12>threshold) { |
RiP | 25:1a71424b05ff | 143 | mystate = STATE_X_NEG; |
RiP | 25:1a71424b05ff | 144 | |
RiP | 25:1a71424b05ff | 145 | } else if (emg22>threshold) { |
RiP | 25:1a71424b05ff | 146 | mystate = STATE_Y_NEG; |
RiP | 25:1a71424b05ff | 147 | } else { |
RiP | 25:1a71424b05ff | 148 | mystate = STATE_PAUZE; |
RiP | 25:1a71424b05ff | 149 | } |
RiP | 25:1a71424b05ff | 150 | } else { |
RiP | 25:1a71424b05ff | 151 | if (emg12>threshold&&emg22>threshold) { |
RiP | 25:1a71424b05ff | 152 | mystate = STATE_XY; |
RiP | 25:1a71424b05ff | 153 | } else if (emg12>threshold) { |
RiP | 25:1a71424b05ff | 154 | mystate = STATE_X; |
RiP | 25:1a71424b05ff | 155 | |
RiP | 25:1a71424b05ff | 156 | } else if (emg22>threshold) { |
RiP | 25:1a71424b05ff | 157 | mystate = STATE_Y; |
RiP | 25:1a71424b05ff | 158 | } else { |
RiP | 25:1a71424b05ff | 159 | mystate = STATE_PAUZE; |
RiP | 25:1a71424b05ff | 160 | } |
RiP | 25:1a71424b05ff | 161 | } |
RiP | 27:1ff7fa636f1c | 162 | |
RiP | 25:1a71424b05ff | 163 | if (buttonflag==true) { |
RiP | 25:1a71424b05ff | 164 | mystate = STATE_CALIBRATION; |
RiP | 25:1a71424b05ff | 165 | } |
RiP | 25:1a71424b05ff | 166 | |
RiP | 25:1a71424b05ff | 167 | if (myoldstate==mystate) { |
RiP | 25:1a71424b05ff | 168 | newcase=false; |
RiP | 28:3b1b29193851 | 169 | newcaseprint=false; |
RiP | 28:3b1b29193851 | 170 | |
RiP | 25:1a71424b05ff | 171 | } else { |
RiP | 25:1a71424b05ff | 172 | newcase=true; |
RiP | 28:3b1b29193851 | 173 | newcaseprint=true; |
RiP | 25:1a71424b05ff | 174 | } |
RiP | 25:1a71424b05ff | 175 | } |
RiP | 25:1a71424b05ff | 176 | |
RiP | 23:54d28f9eef53 | 177 | void sample(states &mystate) |
vsluiter | 2:e314bb3b2d99 | 178 | { |
RiP | 27:1ff7fa636f1c | 179 | states myoldstate=mystate; |
RiP | 23:54d28f9eef53 | 180 | |
RiP | 21:3aecd735319d | 181 | /* Read the emg signals and filter it*/ |
RiP | 22:f38a15e851d2 | 182 | |
RiP | 21:3aecd735319d | 183 | scope.set(0, emg1.read()); //original signal |
RiP | 27:1ff7fa636f1c | 184 | emg02=bqc13.step(fabs(bqc11.step(emg1.read()))); |
RiP | 21:3aecd735319d | 185 | scope.set(1, emg02); |
RiP | 21:3aecd735319d | 186 | /* Read the second emg signal and filter it*/ |
RiP | 21:3aecd735319d | 187 | scope.set(2, emg2.read()); //original signal |
RiP | 27:1ff7fa636f1c | 188 | emg12=bqc23.step(fabs(bqc21.step(emg2.read()))); |
RiP | 21:3aecd735319d | 189 | scope.set(3, emg12); |
RiP | 21:3aecd735319d | 190 | /* Read the third emg signal and filter it*/ |
RiP | 21:3aecd735319d | 191 | scope.set(4, emg3.read()); //original signal |
RiP | 27:1ff7fa636f1c | 192 | emg22=bqc33.step(fabs(bqc31.step(emg3.read()))); |
RiP | 21:3aecd735319d | 193 | scope.set(5, emg22); |
RiP | 29:ac08c1a32c54 | 194 | |
RiP | 22:f38a15e851d2 | 195 | //pc.printf("Hello World!\n"); |
RiP | 21:3aecd735319d | 196 | /* Ensure that enough channels are available (HIDScope scope( 2 )) |
RiP | 21:3aecd735319d | 197 | * Finally, send all channels to the PC at once */ |
vsluiter | 11:ce72ec658a95 | 198 | scope.send(); |
RiP | 21:3aecd735319d | 199 | /* To indicate that the function is working, the LED is toggled */ |
RiP | 25:1a71424b05ff | 200 | |
tomlankhorst | 18:21d8e7a81cf5 | 201 | led = !led; |
RiP | 23:54d28f9eef53 | 202 | if (emg02>threshold) { |
RiP | 23:54d28f9eef53 | 203 | if (emg12>threshold&&emg22>threshold) { |
RiP | 24:01b4b51b5dc6 | 204 | mystate = STATE_XY_NEG; |
RiP | 23:54d28f9eef53 | 205 | } else if (emg12>threshold) { |
RiP | 24:01b4b51b5dc6 | 206 | mystate = STATE_X_NEG; |
RiP | 23:54d28f9eef53 | 207 | |
RiP | 23:54d28f9eef53 | 208 | } else if (emg22>threshold) { |
RiP | 24:01b4b51b5dc6 | 209 | mystate = STATE_Y_NEG; |
RiP | 23:54d28f9eef53 | 210 | } else { |
RiP | 24:01b4b51b5dc6 | 211 | mystate = STATE_PAUZE; |
RiP | 23:54d28f9eef53 | 212 | } |
RiP | 22:f38a15e851d2 | 213 | } else { |
RiP | 23:54d28f9eef53 | 214 | if (emg12>threshold&&emg22>threshold) { |
RiP | 24:01b4b51b5dc6 | 215 | mystate = STATE_XY; |
RiP | 23:54d28f9eef53 | 216 | } else if (emg12>threshold) { |
RiP | 24:01b4b51b5dc6 | 217 | mystate = STATE_X; |
RiP | 23:54d28f9eef53 | 218 | |
RiP | 23:54d28f9eef53 | 219 | } else if (emg22>threshold) { |
RiP | 24:01b4b51b5dc6 | 220 | mystate = STATE_Y; |
RiP | 23:54d28f9eef53 | 221 | } else { |
RiP | 24:01b4b51b5dc6 | 222 | mystate = STATE_PAUZE; |
RiP | 23:54d28f9eef53 | 223 | } |
RiP | 22:f38a15e851d2 | 224 | } |
RiP | 23:54d28f9eef53 | 225 | |
RiP | 24:01b4b51b5dc6 | 226 | if (buttonflag==true) { |
RiP | 24:01b4b51b5dc6 | 227 | mystate = STATE_CALIBRATION; |
RiP | 24:01b4b51b5dc6 | 228 | } |
RiP | 24:01b4b51b5dc6 | 229 | |
RiP | 27:1ff7fa636f1c | 230 | if (myoldstate==mystate) { |
RiP | 28:3b1b29193851 | 231 | newcaseprint=false; |
RiP | 27:1ff7fa636f1c | 232 | newcase=false; |
RiP | 27:1ff7fa636f1c | 233 | } else { |
RiP | 28:3b1b29193851 | 234 | newcaseprint=true; |
RiP | 27:1ff7fa636f1c | 235 | newcase=true; |
RiP | 27:1ff7fa636f1c | 236 | } |
vsluiter | 2:e314bb3b2d99 | 237 | } |
vsluiter | 0:32bb76391d89 | 238 | |
RiP | 27:1ff7fa636f1c | 239 | void change_x(int direction) |
RiP | 27:1ff7fa636f1c | 240 | { |
RiP | 30:7cc564d38bc4 | 241 | ref_x=ref_x+speed*direction; |
RiP | 27:1ff7fa636f1c | 242 | |
RiP | 27:1ff7fa636f1c | 243 | } |
RiP | 27:1ff7fa636f1c | 244 | |
RiP | 27:1ff7fa636f1c | 245 | void change_y(int direction) |
RiP | 27:1ff7fa636f1c | 246 | { |
RiP | 30:7cc564d38bc4 | 247 | ref_y=ref_y+speed*direction; |
RiP | 27:1ff7fa636f1c | 248 | |
RiP | 27:1ff7fa636f1c | 249 | } |
RiP | 27:1ff7fa636f1c | 250 | void change_xy(int direction) |
RiP | 27:1ff7fa636f1c | 251 | { |
RiP | 30:7cc564d38bc4 | 252 | ref_x=ref_x+speed*direction; |
RiP | 30:7cc564d38bc4 | 253 | ref_y=ref_y+speed*direction; |
RiP | 27:1ff7fa636f1c | 254 | } |
RiP | 27:1ff7fa636f1c | 255 | |
RiP | 27:1ff7fa636f1c | 256 | void my_pos() |
RiP | 27:1ff7fa636f1c | 257 | { |
RiP | 27:1ff7fa636f1c | 258 | pc.printf("x_pos=%.4f\ty_pos=%.4f\n\r",ref_x,ref_y); |
RiP | 27:1ff7fa636f1c | 259 | |
RiP | 27:1ff7fa636f1c | 260 | } |
RiP | 26:91d48c0b722d | 261 | |
RiP | 29:ac08c1a32c54 | 262 | void print_state() |
RiP | 28:3b1b29193851 | 263 | { |
RiP | 28:3b1b29193851 | 264 | // switch, case |
RiP | 28:3b1b29193851 | 265 | if (newcaseprint==true) { |
RiP | 28:3b1b29193851 | 266 | switch (mystate) { |
RiP | 28:3b1b29193851 | 267 | case STATE_CALIBRATION : { // calibration |
RiP | 28:3b1b29193851 | 268 | pc.printf("calibration\n\r"); |
RiP | 28:3b1b29193851 | 269 | break; |
RiP | 28:3b1b29193851 | 270 | } |
RiP | 28:3b1b29193851 | 271 | case STATE_X : // run |
RiP | 29:ac08c1a32c54 | 272 | pc.printf("X\n\r"); |
RiP | 28:3b1b29193851 | 273 | break; |
RiP | 28:3b1b29193851 | 274 | case STATE_X_NEG : // run |
RiP | 29:ac08c1a32c54 | 275 | pc.printf("Xneg\n\r"); |
RiP | 28:3b1b29193851 | 276 | break; |
RiP | 28:3b1b29193851 | 277 | case STATE_Y : // execute mode 1 |
RiP | 29:ac08c1a32c54 | 278 | pc.printf("Y\n\r"); |
RiP | 28:3b1b29193851 | 279 | break; |
RiP | 28:3b1b29193851 | 280 | case STATE_Y_NEG : // execute mode 1 |
RiP | 29:ac08c1a32c54 | 281 | pc.printf("Yneg\n\r"); |
RiP | 28:3b1b29193851 | 282 | break; |
RiP | 28:3b1b29193851 | 283 | case STATE_XY : // execute mode 2 |
RiP | 29:ac08c1a32c54 | 284 | pc.printf("XY\n\r"); |
RiP | 28:3b1b29193851 | 285 | break; |
RiP | 28:3b1b29193851 | 286 | case STATE_XY_NEG : // execute mode 2 |
RiP | 29:ac08c1a32c54 | 287 | pc.printf("XYneg\n\r"); |
RiP | 28:3b1b29193851 | 288 | break; |
RiP | 28:3b1b29193851 | 289 | case STATE_PAUZE : // default |
RiP | 29:ac08c1a32c54 | 290 | pc.printf("PAUZE\n\r"); |
RiP | 28:3b1b29193851 | 291 | break; |
RiP | 28:3b1b29193851 | 292 | } |
RiP | 28:3b1b29193851 | 293 | } |
RiP | 28:3b1b29193851 | 294 | |
RiP | 28:3b1b29193851 | 295 | } |
RiP | 28:3b1b29193851 | 296 | |
RiP | 29:ac08c1a32c54 | 297 | void change_ref() |
RiP | 29:ac08c1a32c54 | 298 | { |
RiP | 29:ac08c1a32c54 | 299 | if (change_ref_timer==true) { |
RiP | 29:ac08c1a32c54 | 300 | if (mystate == STATE_X) { |
RiP | 29:ac08c1a32c54 | 301 | change_x(1); |
RiP | 29:ac08c1a32c54 | 302 | } else if (mystate == STATE_X_NEG) { |
RiP | 29:ac08c1a32c54 | 303 | change_x(-1); |
RiP | 29:ac08c1a32c54 | 304 | } else if (mystate == STATE_Y) { |
RiP | 29:ac08c1a32c54 | 305 | change_y(1); |
RiP | 29:ac08c1a32c54 | 306 | } else if (mystate == STATE_Y_NEG) { |
RiP | 29:ac08c1a32c54 | 307 | change_y(-1); |
RiP | 29:ac08c1a32c54 | 308 | } else if (mystate == STATE_XY) { |
RiP | 29:ac08c1a32c54 | 309 | change_xy(1); |
RiP | 29:ac08c1a32c54 | 310 | } else if (mystate == STATE_XY_NEG) { |
RiP | 29:ac08c1a32c54 | 311 | change_xy(-1); |
RiP | 29:ac08c1a32c54 | 312 | } |
RiP | 29:ac08c1a32c54 | 313 | change_ref_timer=false; |
RiP | 29:ac08c1a32c54 | 314 | } |
RiP | 29:ac08c1a32c54 | 315 | } |
RiP | 29:ac08c1a32c54 | 316 | |
RiP | 29:ac08c1a32c54 | 317 | |
vsluiter | 0:32bb76391d89 | 318 | int main() |
RiP | 21:3aecd735319d | 319 | { |
RiP | 29:ac08c1a32c54 | 320 | pc.printf("RESET\n\r"); |
RiP | 22:f38a15e851d2 | 321 | pc.baud(115200); |
RiP | 22:f38a15e851d2 | 322 | |
RiP | 21:3aecd735319d | 323 | //de biquad chains instellen |
RiP | 21:3aecd735319d | 324 | bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ); |
RiP | 21:3aecd735319d | 325 | bqc13.add( &bq131); |
RiP | 21:3aecd735319d | 326 | bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ); |
RiP | 21:3aecd735319d | 327 | bqc23.add( &bq231); |
RiP | 21:3aecd735319d | 328 | bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ); |
RiP | 21:3aecd735319d | 329 | bqc33.add( &bq331); |
RiP | 21:3aecd735319d | 330 | /*Attach the 'sample' function to the timer 'sample_timer'. |
RiP | 21:3aecd735319d | 331 | this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz |
vsluiter | 4:8b298dfada81 | 332 | */ |
RiP | 27:1ff7fa636f1c | 333 | pos_timer.attach(&my_pos, 1); |
RiP | 23:54d28f9eef53 | 334 | sample_timer.attach(&sampleflag, samplefreq); |
RiP | 24:01b4b51b5dc6 | 335 | button_calibrate.fall(&buttonflag_go); |
RiP | 24:01b4b51b5dc6 | 336 | |
tomlankhorst | 15:0da764eea774 | 337 | |
RiP | 22:f38a15e851d2 | 338 | while(1) { |
RiP | 22:f38a15e851d2 | 339 | if (sampletimer==true) { |
RiP | 25:1a71424b05ff | 340 | //sample(mystate); |
RiP | 30:7cc564d38bc4 | 341 | sample_button(mystate); |
RiP | 29:ac08c1a32c54 | 342 | print_state(); |
RiP | 29:ac08c1a32c54 | 343 | change_ref(); |
RiP | 29:ac08c1a32c54 | 344 | sampletimer = false; |
RiP | 29:ac08c1a32c54 | 345 | |
RiP | 29:ac08c1a32c54 | 346 | |
RiP | 25:1a71424b05ff | 347 | } |
RiP | 22:f38a15e851d2 | 348 | } |
vsluiter | 0:32bb76391d89 | 349 | } |