EMG filtering; highpass, notch, abs, moving average
Dependencies: HIDScope MODSERIAL- mbed-dsp mbed
Project_main.cpp@42:f45e4dfbc26d, 2014-10-22 (annotated)
- Committer:
- Hooglugt
- Date:
- Wed Oct 22 09:23:09 2014 +0000
- Revision:
- 42:f45e4dfbc26d
- Parent:
- 41:245f33fb2b8b
EMG hoogdoorlaat, notch, abs en 10e orde moving average
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Hooglugt | 23:8d9a623dd713 | 1 | #include "mbed.h" |
Hooglugt | 23:8d9a623dd713 | 2 | #include "MODSERIAL.h" |
Hooglugt | 23:8d9a623dd713 | 3 | #include "HIDScope.h" |
Hooglugt | 40:7e93c2f1c1e9 | 4 | #include "arm_math.h" |
Hooglugt | 29:7523e4a8e000 | 5 | |
Hooglugt | 39:0328b1f16a5a | 6 | #define TIMEB4NEXTCHOICE 1 // sec keuzelampje blijft aan |
Hooglugt | 39:0328b1f16a5a | 7 | #define TIMEBETWEENBLINK 20 // sec voor volgende blink |
Hooglugt | 23:8d9a623dd713 | 8 | |
Hooglugt | 23:8d9a623dd713 | 9 | //Define objects |
Hooglugt | 29:7523e4a8e000 | 10 | AnalogIn emg0(PTB1); //Analog input biceps |
Hooglugt | 40:7e93c2f1c1e9 | 11 | float filemg_bifloat; //filtered emg-waarde biceps |
Hooglugt | 38:277ba1c0693c | 12 | |
Hooglugt | 29:7523e4a8e000 | 13 | AnalogIn emg1(PTB2); //Analog input triceps |
Hooglugt | 40:7e93c2f1c1e9 | 14 | float filemg_trifloat; //filtered emg-waarde triceps |
Hooglugt | 29:7523e4a8e000 | 15 | |
Hooglugt | 38:277ba1c0693c | 16 | Ticker log_timer; |
Hooglugt | 38:277ba1c0693c | 17 | Ticker reset_timer; |
Hooglugt | 38:277ba1c0693c | 18 | MODSERIAL pc(USBTX,USBRX); |
Hooglugt | 39:0328b1f16a5a | 19 | HIDScope scope(2); |
Hooglugt | 29:7523e4a8e000 | 20 | |
Hooglugt | 41:245f33fb2b8b | 21 | arm_biquad_casd_df1_inst_f32 highpass; |
Hooglugt | 41:245f33fb2b8b | 22 | float highpass_const[] = {0.8751821104711265, -1.750364220942253, 0.8751821104711265, 1.7347238224240125 , -0.7660046194604936}; //highpass, Fc: 15 Hz, Fsample: 500Hz, Q = 0.7071 |
Hooglugt | 41:245f33fb2b8b | 23 | float highpass_states[4]; |
Hooglugt | 40:7e93c2f1c1e9 | 24 | |
Hooglugt | 41:245f33fb2b8b | 25 | arm_biquad_casd_df1_inst_f32 notch; |
Hooglugt | 41:245f33fb2b8b | 26 | float notch_const[] = {0.9714498065192796, -1.5718388053127037, 0.9714498065192796, 1.5718388053127037 , -0.9428996130385592}; //notch, Fc: 50 Hz, Fsample: 500Hz, Q = 10 |
Hooglugt | 41:245f33fb2b8b | 27 | float notch_states[4]; |
Hooglugt | 40:7e93c2f1c1e9 | 28 | |
Hooglugt | 41:245f33fb2b8b | 29 | // variables for biceps MAF |
Hooglugt | 41:245f33fb2b8b | 30 | float y0 = 0; |
Hooglugt | 41:245f33fb2b8b | 31 | float y1 = 0; |
Hooglugt | 41:245f33fb2b8b | 32 | float y2 = 0; |
Hooglugt | 41:245f33fb2b8b | 33 | float y3 = 0; |
Hooglugt | 41:245f33fb2b8b | 34 | float y4 = 0; |
Hooglugt | 41:245f33fb2b8b | 35 | float y5 = 0; |
Hooglugt | 41:245f33fb2b8b | 36 | float y6 = 0; |
Hooglugt | 41:245f33fb2b8b | 37 | float y7 = 0; |
Hooglugt | 41:245f33fb2b8b | 38 | float y8 = 0; |
Hooglugt | 41:245f33fb2b8b | 39 | float y9 = 0; |
Hooglugt | 40:7e93c2f1c1e9 | 40 | |
Hooglugt | 41:245f33fb2b8b | 41 | // variables for triceps MAF |
Hooglugt | 41:245f33fb2b8b | 42 | float x0 = 0; |
Hooglugt | 41:245f33fb2b8b | 43 | float x1 = 0; |
Hooglugt | 41:245f33fb2b8b | 44 | float x2 = 0; |
Hooglugt | 41:245f33fb2b8b | 45 | float x3 = 0; |
Hooglugt | 41:245f33fb2b8b | 46 | float x4 = 0; |
Hooglugt | 41:245f33fb2b8b | 47 | float x5 = 0; |
Hooglugt | 41:245f33fb2b8b | 48 | float x6 = 0; |
Hooglugt | 41:245f33fb2b8b | 49 | float x7 = 0; |
Hooglugt | 41:245f33fb2b8b | 50 | float x8 = 0; |
Hooglugt | 41:245f33fb2b8b | 51 | float x9 = 0; |
Hooglugt | 40:7e93c2f1c1e9 | 52 | |
Hooglugt | 29:7523e4a8e000 | 53 | PwmOut red(LED_RED); |
Hooglugt | 23:8d9a623dd713 | 54 | PwmOut green(LED_GREEN); |
Hooglugt | 23:8d9a623dd713 | 55 | PwmOut blue(LED_BLUE); |
Hooglugt | 29:7523e4a8e000 | 56 | |
Hooglugt | 40:7e93c2f1c1e9 | 57 | uint8_t direction = 0; |
Hooglugt | 40:7e93c2f1c1e9 | 58 | uint8_t force = 0; |
Hooglugt | 23:8d9a623dd713 | 59 | |
Hooglugt | 41:245f33fb2b8b | 60 | void looper() |
Hooglugt | 23:8d9a623dd713 | 61 | { |
Hooglugt | 39:0328b1f16a5a | 62 | //put raw emg value of biceps and triceps in emg_bifloat and emg_trifloat, respectively |
Hooglugt | 40:7e93c2f1c1e9 | 63 | float emg_bifloat; //Float voor EMG-waarde biceps |
Hooglugt | 40:7e93c2f1c1e9 | 64 | float emg_trifloat; //Float voor EMG-waarde triceps |
Hooglugt | 38:277ba1c0693c | 65 | emg_bifloat = emg0.read(); // read float value (0..1 = 0..3.3V) biceps |
Hooglugt | 38:277ba1c0693c | 66 | emg_trifloat = emg1.read(); // read float value (0..1 = 0..3.3V) triceps |
Hooglugt | 41:245f33fb2b8b | 67 | |
Hooglugt | 40:7e93c2f1c1e9 | 68 | //process emg biceps |
Hooglugt | 41:245f33fb2b8b | 69 | arm_biquad_cascade_df1_f32(&highpass, &emg_bifloat, &filemg_bifloat, 1 ); |
Hooglugt | 41:245f33fb2b8b | 70 | arm_biquad_cascade_df1_f32(¬ch, &emg_bifloat, &filemg_bifloat, 1 ); |
Hooglugt | 41:245f33fb2b8b | 71 | y0 = fabs(filemg_bifloat); |
Hooglugt | 41:245f33fb2b8b | 72 | float bi_result = y0*0.1 +y1*0.1 + y2*0.1 + y3*0.1 + y4*0.1 + y5*0.1 + y6*0.1 + y7*0.1 + y8*0.1 + y9*0.1; |
Hooglugt | 41:245f33fb2b8b | 73 | y9=y8; |
Hooglugt | 41:245f33fb2b8b | 74 | y8=y7; |
Hooglugt | 41:245f33fb2b8b | 75 | y7=y6; |
Hooglugt | 41:245f33fb2b8b | 76 | y6=y5; |
Hooglugt | 41:245f33fb2b8b | 77 | y5=y4; |
Hooglugt | 41:245f33fb2b8b | 78 | y4=y3; |
Hooglugt | 41:245f33fb2b8b | 79 | y3=y2; |
Hooglugt | 41:245f33fb2b8b | 80 | y2=y1; |
Hooglugt | 41:245f33fb2b8b | 81 | y1=y0; |
Hooglugt | 41:245f33fb2b8b | 82 | |
Hooglugt | 40:7e93c2f1c1e9 | 83 | //process emg triceps |
Hooglugt | 41:245f33fb2b8b | 84 | arm_biquad_cascade_df1_f32(&highpass, &emg_trifloat, &filemg_trifloat, 1 ); |
Hooglugt | 41:245f33fb2b8b | 85 | arm_biquad_cascade_df1_f32(¬ch, &emg_trifloat, &filemg_trifloat, 1 ); |
Hooglugt | 41:245f33fb2b8b | 86 | x0 = fabs(filemg_trifloat); |
Hooglugt | 41:245f33fb2b8b | 87 | float tri_result = x0*0.1 +x1*0.1 + x2*0.1 + x3*0.1 + x4*0.1 + x5*0.1 + x6*0.1 + x7*0.1 + x8*0.1 + x9*0.1; |
Hooglugt | 41:245f33fb2b8b | 88 | x9=x8; |
Hooglugt | 41:245f33fb2b8b | 89 | x8=x7; |
Hooglugt | 41:245f33fb2b8b | 90 | x7=x6; |
Hooglugt | 41:245f33fb2b8b | 91 | x6=x5; |
Hooglugt | 41:245f33fb2b8b | 92 | x5=x4; |
Hooglugt | 41:245f33fb2b8b | 93 | x4=x3; |
Hooglugt | 41:245f33fb2b8b | 94 | x3=x2; |
Hooglugt | 41:245f33fb2b8b | 95 | x2=x1; |
Hooglugt | 41:245f33fb2b8b | 96 | x1=x0; |
Hooglugt | 29:7523e4a8e000 | 97 | |
Hooglugt | 31:d8eaf0ce8517 | 98 | /*send value to PC. Line below is used to prevent buffer overrun */ |
Hooglugt | 41:245f33fb2b8b | 99 | if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) { |
Hooglugt | 41:245f33fb2b8b | 100 | scope.set(0,bi_result); |
Hooglugt | 41:245f33fb2b8b | 101 | scope.set(1,tri_result); |
Hooglugt | 41:245f33fb2b8b | 102 | scope.send(); |
Hooglugt | 29:7523e4a8e000 | 103 | } |
Hooglugt | 23:8d9a623dd713 | 104 | } |
Hooglugt | 23:8d9a623dd713 | 105 | |
Hooglugt | 34:e166187e62ce | 106 | /* |
Hooglugt | 32:aaf01b1bf05d | 107 | void resetlooper() // VRAAG: wat gebeurt er wanneer en resetlooper en looper tegelijkertijd gecalled worden?! |
Hooglugt | 31:d8eaf0ce8517 | 108 | { |
Hooglugt | 41:245f33fb2b8b | 109 | if(filemg_trifloat.read()>0.8 && direction != 0) { //dit is alleen mogelijk wanneer directionchoice is gemaakt |
Hooglugt | 30:5e5098b0cca6 | 110 | direction = 0; |
Hooglugt | 30:5e5098b0cca6 | 111 | force = 0; // WEGHALEN, wanneer in uiteindelijke script na force keuzen niet meer gereset kan worden (voor nu wel handig) |
Hooglugt | 31:d8eaf0ce8517 | 112 | pc.printf("reset "); |
Hooglugt | 31:d8eaf0ce8517 | 113 | } |
Hooglugt | 30:5e5098b0cca6 | 114 | } |
Hooglugt | 41:245f33fb2b8b | 115 | */ |
Hooglugt | 31:d8eaf0ce8517 | 116 | |
Hooglugt | 23:8d9a623dd713 | 117 | int main() |
Hooglugt | 23:8d9a623dd713 | 118 | { |
Hooglugt | 25:ec41b972b250 | 119 | pc.baud(115200); //baudrate instellen |
Hooglugt | 40:7e93c2f1c1e9 | 120 | log_timer.attach(looper, 0.002); // The looper() function will be called every 0.002 seconds (with the ticker object) |
Hooglugt | 41:245f33fb2b8b | 121 | //set up filters |
Hooglugt | 41:245f33fb2b8b | 122 | arm_biquad_cascade_df1_init_f32(¬ch, 1, notch_const, notch_states); |
Hooglugt | 41:245f33fb2b8b | 123 | arm_biquad_cascade_df1_init_f32(&highpass, 1, highpass_const, highpass_states); |
Hooglugt | 41:245f33fb2b8b | 124 | |
Hooglugt | 41:245f33fb2b8b | 125 | |
Hooglugt | 34:e166187e62ce | 126 | // reset_timer.attach(resetlooper, 0.1); // |
Hooglugt | 32:aaf01b1bf05d | 127 | |
Hooglugt | 37:2d248e64b745 | 128 | goto directionchoice; // goes to first while(1) for the deciding the direction |
Hooglugt | 37:2d248e64b745 | 129 | |
Hooglugt | 37:2d248e64b745 | 130 | while(1) { //Loop keuze DIRECTION |
Hooglugt | 37:2d248e64b745 | 131 | directionchoice: |
Hooglugt | 23:8d9a623dd713 | 132 | for(int i=1; i<4; i++) { |
Hooglugt | 23:8d9a623dd713 | 133 | if(i==1) { //red |
Hooglugt | 23:8d9a623dd713 | 134 | red=0; |
Hooglugt | 23:8d9a623dd713 | 135 | green=1; |
Hooglugt | 23:8d9a623dd713 | 136 | blue=1; |
Hooglugt | 37:2d248e64b745 | 137 | for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { |
Hooglugt | 40:7e93c2f1c1e9 | 138 | if(filemg_bifloat>0.8) { // 0.8 klopt niet als grenswaarde. #nofilter |
Hooglugt | 25:ec41b972b250 | 139 | direction = 1; |
Hooglugt | 37:2d248e64b745 | 140 | red=1; |
Hooglugt | 37:2d248e64b745 | 141 | green = 0; |
Hooglugt | 36:af949aaaba01 | 142 | blue = 0; |
Hooglugt | 37:2d248e64b745 | 143 | pc.printf("links "); |
Hooglugt | 37:2d248e64b745 | 144 | wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie |
Hooglugt | 37:2d248e64b745 | 145 | goto forcechoice; // goes to second while(1) for the deciding the force |
Hooglugt | 23:8d9a623dd713 | 146 | } else { |
Hooglugt | 23:8d9a623dd713 | 147 | wait(0.1); |
Hooglugt | 23:8d9a623dd713 | 148 | } |
Hooglugt | 23:8d9a623dd713 | 149 | } |
Hooglugt | 23:8d9a623dd713 | 150 | } |
Hooglugt | 23:8d9a623dd713 | 151 | if(i==2) { //green |
Hooglugt | 23:8d9a623dd713 | 152 | red =1; |
Hooglugt | 23:8d9a623dd713 | 153 | green=0; |
Hooglugt | 23:8d9a623dd713 | 154 | blue=1; |
Hooglugt | 37:2d248e64b745 | 155 | for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { |
Hooglugt | 40:7e93c2f1c1e9 | 156 | if(filemg_bifloat>0.8) { //0.8 klopt niet als grenswaarde. #nofilter |
Hooglugt | 25:ec41b972b250 | 157 | direction = 2; |
Hooglugt | 37:2d248e64b745 | 158 | red=0; |
Hooglugt | 29:7523e4a8e000 | 159 | green = 1; |
Hooglugt | 37:2d248e64b745 | 160 | blue = 0; |
Hooglugt | 37:2d248e64b745 | 161 | pc.printf("mid "); |
Hooglugt | 37:2d248e64b745 | 162 | wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie |
Hooglugt | 23:8d9a623dd713 | 163 | goto forcechoice; |
Hooglugt | 23:8d9a623dd713 | 164 | } else { |
Hooglugt | 23:8d9a623dd713 | 165 | wait(0.1); |
Hooglugt | 23:8d9a623dd713 | 166 | } |
Hooglugt | 23:8d9a623dd713 | 167 | } |
Hooglugt | 23:8d9a623dd713 | 168 | } |
Hooglugt | 23:8d9a623dd713 | 169 | if(i==3) { //blue |
Hooglugt | 23:8d9a623dd713 | 170 | red=1; |
Hooglugt | 23:8d9a623dd713 | 171 | green=1; |
Hooglugt | 23:8d9a623dd713 | 172 | blue=0; |
Hooglugt | 37:2d248e64b745 | 173 | for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { |
Hooglugt | 40:7e93c2f1c1e9 | 174 | if(filemg_bifloat>0.8) { //0.8 klopt niet als grenswaarde. #nofilter |
Hooglugt | 23:8d9a623dd713 | 175 | direction = 3; |
Hooglugt | 37:2d248e64b745 | 176 | red=0; |
Hooglugt | 29:7523e4a8e000 | 177 | green = 0; |
Hooglugt | 37:2d248e64b745 | 178 | blue = 1; |
Hooglugt | 37:2d248e64b745 | 179 | pc.printf("rechts "); |
Hooglugt | 37:2d248e64b745 | 180 | wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie |
Hooglugt | 23:8d9a623dd713 | 181 | goto forcechoice; |
Hooglugt | 23:8d9a623dd713 | 182 | } else { |
Hooglugt | 23:8d9a623dd713 | 183 | wait(0.1); |
Hooglugt | 23:8d9a623dd713 | 184 | } |
Hooglugt | 23:8d9a623dd713 | 185 | } |
Hooglugt | 23:8d9a623dd713 | 186 | } |
Hooglugt | 25:ec41b972b250 | 187 | } |
Hooglugt | 23:8d9a623dd713 | 188 | } |
Hooglugt | 37:2d248e64b745 | 189 | while(1) { //Loop keuze FORCE |
Hooglugt | 25:ec41b972b250 | 190 | forcechoice: |
Hooglugt | 37:2d248e64b745 | 191 | for(int j=1; j<4; j++) { |
Hooglugt | 37:2d248e64b745 | 192 | if(j==1) { //red |
Hooglugt | 37:2d248e64b745 | 193 | red=0; |
Hooglugt | 37:2d248e64b745 | 194 | green=1; |
Hooglugt | 37:2d248e64b745 | 195 | blue=1; |
Hooglugt | 37:2d248e64b745 | 196 | if(direction==0) { //if statement die controleert of direction 0 is (dus of triceps gereset is) |
Hooglugt | 37:2d248e64b745 | 197 | goto directionchoice; |
Hooglugt | 37:2d248e64b745 | 198 | } |
Hooglugt | 37:2d248e64b745 | 199 | for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { |
Hooglugt | 40:7e93c2f1c1e9 | 200 | if(filemg_bifloat>0.8) { // 0.8 klopt niet als grenswaarde. #nofilter |
Hooglugt | 37:2d248e64b745 | 201 | force = 1; |
Hooglugt | 37:2d248e64b745 | 202 | red=1; |
Hooglugt | 37:2d248e64b745 | 203 | green = 0; |
Hooglugt | 37:2d248e64b745 | 204 | blue = 0; |
Hooglugt | 37:2d248e64b745 | 205 | pc.printf("zwak "); |
Hooglugt | 37:2d248e64b745 | 206 | wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie |
Hooglugt | 37:2d248e64b745 | 207 | goto choicesmade; |
Hooglugt | 37:2d248e64b745 | 208 | } else { |
Hooglugt | 37:2d248e64b745 | 209 | wait(0.1); |
Hooglugt | 23:8d9a623dd713 | 210 | } |
Hooglugt | 23:8d9a623dd713 | 211 | } |
Hooglugt | 37:2d248e64b745 | 212 | } |
Hooglugt | 37:2d248e64b745 | 213 | if(j==2) { //green |
Hooglugt | 37:2d248e64b745 | 214 | red =1; |
Hooglugt | 37:2d248e64b745 | 215 | green=0; |
Hooglugt | 37:2d248e64b745 | 216 | blue=1; |
Hooglugt | 37:2d248e64b745 | 217 | if(direction==0) { //if statement die controleert of direction 0 is (dus of triceps gereset is) |
Hooglugt | 37:2d248e64b745 | 218 | goto directionchoice; |
Hooglugt | 37:2d248e64b745 | 219 | } |
Hooglugt | 37:2d248e64b745 | 220 | for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { |
Hooglugt | 40:7e93c2f1c1e9 | 221 | if(filemg_bifloat>0.8) { //0.8 klopt niet als grenswaarde. #nofilter |
Hooglugt | 37:2d248e64b745 | 222 | force = 2; |
Hooglugt | 37:2d248e64b745 | 223 | red=0; |
Hooglugt | 37:2d248e64b745 | 224 | green = 1; |
Hooglugt | 37:2d248e64b745 | 225 | blue = 0; |
Hooglugt | 37:2d248e64b745 | 226 | pc.printf("normaal "); |
Hooglugt | 37:2d248e64b745 | 227 | wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie |
Hooglugt | 37:2d248e64b745 | 228 | goto choicesmade; |
Hooglugt | 37:2d248e64b745 | 229 | } else { |
Hooglugt | 37:2d248e64b745 | 230 | wait(0.1); |
Hooglugt | 23:8d9a623dd713 | 231 | } |
Hooglugt | 23:8d9a623dd713 | 232 | } |
Hooglugt | 37:2d248e64b745 | 233 | } |
Hooglugt | 37:2d248e64b745 | 234 | if(j==3) { //blue |
Hooglugt | 37:2d248e64b745 | 235 | red=1; |
Hooglugt | 37:2d248e64b745 | 236 | green=1; |
Hooglugt | 37:2d248e64b745 | 237 | blue=0; |
Hooglugt | 37:2d248e64b745 | 238 | if(direction==0) { //if statement die controleert of direction 0 is (dus of triceps gereset is) |
Hooglugt | 37:2d248e64b745 | 239 | goto directionchoice; |
Hooglugt | 37:2d248e64b745 | 240 | } |
Hooglugt | 37:2d248e64b745 | 241 | for (int lag=0; lag<TIMEBETWEENBLINK; lag++) { |
Hooglugt | 40:7e93c2f1c1e9 | 242 | if(filemg_bifloat>0.8) { //0.8 klopt niet als grenswaarde. #nofilter |
Hooglugt | 37:2d248e64b745 | 243 | force = 3; |
Hooglugt | 37:2d248e64b745 | 244 | red=0; |
Hooglugt | 37:2d248e64b745 | 245 | green = 0; |
Hooglugt | 37:2d248e64b745 | 246 | blue = 1; |
Hooglugt | 37:2d248e64b745 | 247 | pc.printf("sterk "); |
Hooglugt | 37:2d248e64b745 | 248 | wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie |
Hooglugt | 37:2d248e64b745 | 249 | goto choicesmade; |
Hooglugt | 37:2d248e64b745 | 250 | } else { |
Hooglugt | 37:2d248e64b745 | 251 | wait(0.1); |
Hooglugt | 23:8d9a623dd713 | 252 | } |
Hooglugt | 23:8d9a623dd713 | 253 | } |
Hooglugt | 23:8d9a623dd713 | 254 | } |
Hooglugt | 23:8d9a623dd713 | 255 | } |
Hooglugt | 37:2d248e64b745 | 256 | } |
Hooglugt | 37:2d248e64b745 | 257 | choicesmade: |
Hooglugt | 33:a78ec776dfd5 | 258 | |
Hooglugt | 40:7e93c2f1c1e9 | 259 | red = 0; |
Hooglugt | 40:7e93c2f1c1e9 | 260 | green = 0; |
Hooglugt | 40:7e93c2f1c1e9 | 261 | blue = 0; // wit lampje |
Hooglugt | 40:7e93c2f1c1e9 | 262 | |
Hooglugt | 37:2d248e64b745 | 263 | /* Vanaf hier komt de aansturing van de motor (inclusief de controller)*/ |
Hooglugt | 37:2d248e64b745 | 264 | |
Hooglugt | 33:a78ec776dfd5 | 265 | |
Hooglugt | 25:ec41b972b250 | 266 | if(direction == 1 && force == 1) { // links zwak |
Hooglugt | 39:0328b1f16a5a | 267 | pc.printf("links zwak "); |
Hooglugt | 40:7e93c2f1c1e9 | 268 | wait(2); |
Hooglugt | 25:ec41b972b250 | 269 | } |
Hooglugt | 25:ec41b972b250 | 270 | if(direction == 1 && force == 2) { // links normaal |
Hooglugt | 39:0328b1f16a5a | 271 | pc.printf("links normaal "); |
Hooglugt | 25:ec41b972b250 | 272 | } |
Hooglugt | 25:ec41b972b250 | 273 | if(direction == 1 && force == 3) { // links sterk |
Hooglugt | 39:0328b1f16a5a | 274 | pc.printf("links sterk "); |
Hooglugt | 25:ec41b972b250 | 275 | } |
Hooglugt | 25:ec41b972b250 | 276 | if(direction == 2 && force == 1) { // mid zwak |
Hooglugt | 39:0328b1f16a5a | 277 | pc.printf("mid zwak "); |
Hooglugt | 25:ec41b972b250 | 278 | } |
Hooglugt | 25:ec41b972b250 | 279 | if(direction == 2 && force == 2) { // mid normaal |
Hooglugt | 39:0328b1f16a5a | 280 | pc.printf("mid normaal "); |
Hooglugt | 25:ec41b972b250 | 281 | } |
Hooglugt | 25:ec41b972b250 | 282 | if(direction == 2 && force == 3) { // mid sterk |
Hooglugt | 39:0328b1f16a5a | 283 | pc.printf("mid sterk "); |
Hooglugt | 25:ec41b972b250 | 284 | } |
Hooglugt | 25:ec41b972b250 | 285 | if(direction == 3 && force == 1) { // rechts zwak |
Hooglugt | 39:0328b1f16a5a | 286 | pc.printf("rechts zwak "); |
Hooglugt | 25:ec41b972b250 | 287 | } |
Hooglugt | 25:ec41b972b250 | 288 | if(direction == 3 && force == 2) { // rechts normaal |
Hooglugt | 39:0328b1f16a5a | 289 | pc.printf("rechts normaal "); |
Hooglugt | 25:ec41b972b250 | 290 | } |
Hooglugt | 25:ec41b972b250 | 291 | if(direction == 3 && force == 3) { // rechts sterk |
Hooglugt | 39:0328b1f16a5a | 292 | pc.printf("rechts sterk "); |
Hooglugt | 25:ec41b972b250 | 293 | } |
Hooglugt | 33:a78ec776dfd5 | 294 | if(direction == 0 || force == 0) { // wanneer de triceps in de korte tijd is aangespannen nadat beide keuzes zijn gemaakt |
Hooglugt | 39:0328b1f16a5a | 295 | pc.printf("error "); |
Hooglugt | 33:a78ec776dfd5 | 296 | // mogelijkheid om een goto te maken naar directionchoice (opzich wel handig) |
Hooglugt | 32:aaf01b1bf05d | 297 | } |
Hooglugt | 41:245f33fb2b8b | 298 | |
Hooglugt | 25:ec41b972b250 | 299 | } |