dit is em
Dependencies: mbed
main.cpp@11:aac90fca0290, 2017-05-09 (annotated)
- Committer:
- bjorntukkertje
- Date:
- Tue May 09 13:59:06 2017 +0000
- Revision:
- 11:aac90fca0290
- Parent:
- 10:18116d979d61
- Child:
- 12:2f3afa8d0ddc
met zoek sensor;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bjorntukkertje | 0:9756a10feb5f | 1 | #include "mbed.h" |
bjorntukkertje | 0:9756a10feb5f | 2 | |
bjorntukkertje | 11:aac90fca0290 | 3 | PwmOut MotorL1 (D13); |
bjorntukkertje | 11:aac90fca0290 | 4 | PwmOut MotorL2 (A0); |
bjorntukkertje | 11:aac90fca0290 | 5 | PwmOut MotorR3 (D12); |
bjorntukkertje | 11:aac90fca0290 | 6 | PwmOut MotorR4 (D11); |
bjorntukkertje | 6:6659a8ada914 | 7 | AnalogIn Sensor1 (A1); |
bjorntukkertje | 6:6659a8ada914 | 8 | AnalogIn Sensor2 (A2); |
bjorntukkertje | 6:6659a8ada914 | 9 | AnalogIn Sensor3 (A3); |
bjorntukkertje | 6:6659a8ada914 | 10 | AnalogIn Sensor4 (A4); |
bjorntukkertje | 11:aac90fca0290 | 11 | AnalogIn zoeksensor (A5); |
bjorntukkertje | 10:18116d979d61 | 12 | DigitalOut steppera (D2); |
bjorntukkertje | 10:18116d979d61 | 13 | DigitalOut stepperb (D3); |
bjorntukkertje | 10:18116d979d61 | 14 | DigitalOut stepperc (D4); |
bjorntukkertje | 10:18116d979d61 | 15 | DigitalOut stepperd (D5); |
bjorntukkertje | 4:038346651cc0 | 16 | |
bjorntukkertje | 0:9756a10feb5f | 17 | |
bjorntukkertje | 10:18116d979d61 | 18 | Ticker lidar; |
bjorntukkertje | 10:18116d979d61 | 19 | Ticker serial; |
bjorntukkertje | 10:18116d979d61 | 20 | |
bjorntukkertje | 10:18116d979d61 | 21 | Serial pc(USBTX, USBRX); |
bjorntukkertje | 0:9756a10feb5f | 22 | |
bjorntukkertje | 11:aac90fca0290 | 23 | int afstand = zoeksensor; |
bjorntukkertje | 11:aac90fca0290 | 24 | |
bjorntukkertje | 10:18116d979d61 | 25 | int stapmode = 0; |
bjorntukkertje | 10:18116d979d61 | 26 | int position = 0; |
bjorntukkertje | 10:18116d979d61 | 27 | int pos; |
bjorntukkertje | 10:18116d979d61 | 28 | |
bjorntukkertje | 10:18116d979d61 | 29 | // poar neemn |
bjorntukkertje | 10:18116d979d61 | 30 | // twee poar neemn |
bjorntukkertje | 10:18116d979d61 | 31 | // twee tettn in n envelop |
bjorntukkertje | 10:18116d979d61 | 32 | |
bjorntukkertje | 10:18116d979d61 | 33 | int stepper(int stapmode) |
bjorntukkertje | 10:18116d979d61 | 34 | { |
bjorntukkertje | 10:18116d979d61 | 35 | switch (stapmode) { |
bjorntukkertje | 10:18116d979d61 | 36 | case 0: |
bjorntukkertje | 10:18116d979d61 | 37 | steppera = 1; |
bjorntukkertje | 10:18116d979d61 | 38 | stepperb = 0; |
bjorntukkertje | 10:18116d979d61 | 39 | stepperc = 1; |
bjorntukkertje | 10:18116d979d61 | 40 | stepperd = 0; |
bjorntukkertje | 10:18116d979d61 | 41 | pos ++; |
bjorntukkertje | 10:18116d979d61 | 42 | break; |
bjorntukkertje | 10:18116d979d61 | 43 | case 1: |
bjorntukkertje | 10:18116d979d61 | 44 | steppera = 1; |
bjorntukkertje | 10:18116d979d61 | 45 | stepperb = 0; |
bjorntukkertje | 10:18116d979d61 | 46 | stepperc = 0; |
bjorntukkertje | 10:18116d979d61 | 47 | stepperd = 1; |
bjorntukkertje | 10:18116d979d61 | 48 | pos ++; |
bjorntukkertje | 10:18116d979d61 | 49 | break; |
bjorntukkertje | 10:18116d979d61 | 50 | case 2: |
bjorntukkertje | 10:18116d979d61 | 51 | steppera = 0; |
bjorntukkertje | 10:18116d979d61 | 52 | stepperb = 1; |
bjorntukkertje | 10:18116d979d61 | 53 | stepperc = 0; |
bjorntukkertje | 10:18116d979d61 | 54 | stepperd = 1; |
bjorntukkertje | 10:18116d979d61 | 55 | pos ++; |
bjorntukkertje | 10:18116d979d61 | 56 | break; |
bjorntukkertje | 10:18116d979d61 | 57 | case 3: |
bjorntukkertje | 10:18116d979d61 | 58 | steppera = 0; |
bjorntukkertje | 10:18116d979d61 | 59 | stepperb = 1; |
bjorntukkertje | 10:18116d979d61 | 60 | stepperc = 1; |
bjorntukkertje | 10:18116d979d61 | 61 | stepperd = 0; |
bjorntukkertje | 10:18116d979d61 | 62 | pos ++; |
bjorntukkertje | 10:18116d979d61 | 63 | break; |
bjorntukkertje | 10:18116d979d61 | 64 | ; |
bjorntukkertje | 10:18116d979d61 | 65 | } |
bjorntukkertje | 10:18116d979d61 | 66 | if (pos > 1000) { //na volledige rotatie ga naar nul |
bjorntukkertje | 10:18116d979d61 | 67 | pos = 0; |
bjorntukkertje | 10:18116d979d61 | 68 | } |
bjorntukkertje | 10:18116d979d61 | 69 | return pos; |
bjorntukkertje | 10:18116d979d61 | 70 | |
bjorntukkertje | 10:18116d979d61 | 71 | } |
bjorntukkertje | 10:18116d979d61 | 72 | |
bjorntukkertje | 10:18116d979d61 | 73 | void run_serial() //deze functie word periodiek aangeroepen om de stappenmotor te draaien en de afstand te meten |
bjorntukkertje | 10:18116d979d61 | 74 | { |
bjorntukkertje | 10:18116d979d61 | 75 | |
bjorntukkertje | 10:18116d979d61 | 76 | pc.printf("%d\n",position); |
bjorntukkertje | 10:18116d979d61 | 77 | } |
bjorntukkertje | 10:18116d979d61 | 78 | |
bjorntukkertje | 10:18116d979d61 | 79 | void run_lidar() //deze functie word periodiek aangeroepen om de stappenmotor te draaien en de afstand te meten |
bjorntukkertje | 10:18116d979d61 | 80 | { |
bjorntukkertje | 10:18116d979d61 | 81 | stapmode ++; |
bjorntukkertje | 10:18116d979d61 | 82 | if (stapmode > 3) { |
bjorntukkertje | 10:18116d979d61 | 83 | stapmode = 0; |
bjorntukkertje | 10:18116d979d61 | 84 | } |
bjorntukkertje | 10:18116d979d61 | 85 | position = stepper(stapmode); |
bjorntukkertje | 10:18116d979d61 | 86 | } |
bjorntukkertje | 10:18116d979d61 | 87 | |
bjorntukkertje | 8:ec7b7c9ad07e | 88 | |
bjorntukkertje | 0:9756a10feb5f | 89 | int main() { |
bjorntukkertje | 10:18116d979d61 | 90 | lidar.attach(&run_lidar, 0.0001); |
bjorntukkertje | 10:18116d979d61 | 91 | serial.attach(&run_serial, 0.2); |
bjorntukkertje | 0:9756a10feb5f | 92 | while(1) { |
bjorntukkertje | 8:ec7b7c9ad07e | 93 | if ( Sensor1 >= 1) { |
bjorntukkertje | 11:aac90fca0290 | 94 | MotorL1 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 95 | MotorL2 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 96 | MotorR3 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 97 | MotorR4 = 1 ; |
bjorntukkertje | 8:ec7b7c9ad07e | 98 | wait_ms (500); |
bjorntukkertje | 11:aac90fca0290 | 99 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 100 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 101 | MotorR3 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 102 | MotorR4 = 1 ; |
bjorntukkertje | 8:ec7b7c9ad07e | 103 | wait_ms (1000); |
bjorntukkertje | 8:ec7b7c9ad07e | 104 | } |
bjorntukkertje | 8:ec7b7c9ad07e | 105 | if (Sensor2 >= 1) { |
bjorntukkertje | 11:aac90fca0290 | 106 | MotorL1 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 107 | MotorL2 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 108 | MotorR3 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 109 | MotorR4 = 1 ; |
bjorntukkertje | 6:6659a8ada914 | 110 | wait_ms (500); |
bjorntukkertje | 11:aac90fca0290 | 111 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 112 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 113 | MotorR3 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 114 | MotorR4 = 1 ; |
bjorntukkertje | 6:6659a8ada914 | 115 | wait_ms (1000); |
bjorntukkertje | 6:6659a8ada914 | 116 | } |
bjorntukkertje | 8:ec7b7c9ad07e | 117 | if (Sensor3 >= 1) { |
bjorntukkertje | 11:aac90fca0290 | 118 | MotorL1 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 119 | MotorL2 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 120 | MotorR3 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 121 | MotorR4 = 1 ; |
bjorntukkertje | 8:ec7b7c9ad07e | 122 | wait_ms (500); |
bjorntukkertje | 11:aac90fca0290 | 123 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 124 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 125 | MotorR3 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 126 | MotorR4 = 1 ; |
bjorntukkertje | 8:ec7b7c9ad07e | 127 | wait_ms (1000); |
bjorntukkertje | 8:ec7b7c9ad07e | 128 | } |
bjorntukkertje | 8:ec7b7c9ad07e | 129 | if (Sensor4 >= 1) { |
bjorntukkertje | 11:aac90fca0290 | 130 | MotorL1 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 131 | MotorL2 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 132 | MotorR3 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 133 | MotorR4 = 1 ; |
bjorntukkertje | 8:ec7b7c9ad07e | 134 | wait_ms (500); |
bjorntukkertje | 11:aac90fca0290 | 135 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 136 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 137 | MotorR3 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 138 | MotorR4 = 1 ; |
bjorntukkertje | 8:ec7b7c9ad07e | 139 | wait_ms (1000); |
bjorntukkertje | 8:ec7b7c9ad07e | 140 | } |
bjorntukkertje | 2:5c11199e4ff2 | 141 | else { |
bjorntukkertje | 11:aac90fca0290 | 142 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 143 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 144 | MotorR3 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 145 | MotorR4 = 0 ; |
bjorntukkertje | 2:5c11199e4ff2 | 146 | } |
bjorntukkertje | 11:aac90fca0290 | 147 | |
bjorntukkertje | 0:9756a10feb5f | 148 | |
bjorntukkertje | 0:9756a10feb5f | 149 | } |
bjorntukkertje | 11:aac90fca0290 | 150 | while (1){ |
bjorntukkertje | 11:aac90fca0290 | 151 | |
bjorntukkertje | 11:aac90fca0290 | 152 | if (afstand > 0.25 && pos > 17 && pos < 45) { |
bjorntukkertje | 11:aac90fca0290 | 153 | // 10 graden |
bjorntukkertje | 11:aac90fca0290 | 154 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 155 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 156 | MotorR3 = 0.95 ; |
bjorntukkertje | 11:aac90fca0290 | 157 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 158 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 159 | } |
bjorntukkertje | 11:aac90fca0290 | 160 | if (afstand > 0.25 && pos >= 45&& pos < 72) { |
bjorntukkertje | 11:aac90fca0290 | 161 | // 20 graden |
bjorntukkertje | 11:aac90fca0290 | 162 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 163 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 164 | MotorR3 = 0.9 ; |
bjorntukkertje | 11:aac90fca0290 | 165 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 166 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 167 | } |
bjorntukkertje | 11:aac90fca0290 | 168 | if (afstand > 0.25 && pos >= 72 && pos < 100) { |
bjorntukkertje | 11:aac90fca0290 | 169 | // 30 graden |
bjorntukkertje | 11:aac90fca0290 | 170 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 171 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 172 | MotorR3 = 0.85 ; |
bjorntukkertje | 11:aac90fca0290 | 173 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 174 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 175 | } |
bjorntukkertje | 11:aac90fca0290 | 176 | if (afstand > 0.25 && pos >= 100 && pos < 128) { |
bjorntukkertje | 11:aac90fca0290 | 177 | // 40 graden |
bjorntukkertje | 11:aac90fca0290 | 178 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 179 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 180 | MotorR3 = 0.8 ; |
bjorntukkertje | 11:aac90fca0290 | 181 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 182 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 183 | } |
bjorntukkertje | 11:aac90fca0290 | 184 | if (afstand > 0.25 && pos >= 128 && pos < 156) { |
bjorntukkertje | 11:aac90fca0290 | 185 | // 50 graden |
bjorntukkertje | 11:aac90fca0290 | 186 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 187 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 188 | MotorR3 = 0.75 ; |
bjorntukkertje | 11:aac90fca0290 | 189 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 190 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 191 | } |
bjorntukkertje | 11:aac90fca0290 | 192 | if (afstand > 0.25 && pos >= 156 && pos < 183) { |
bjorntukkertje | 11:aac90fca0290 | 193 | // 60 graden |
bjorntukkertje | 11:aac90fca0290 | 194 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 195 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 196 | MotorR3 = 0.7 ; |
bjorntukkertje | 11:aac90fca0290 | 197 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 198 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 199 | } |
bjorntukkertje | 11:aac90fca0290 | 200 | if (afstand > 0.25 && pos >= 183 && pos < 211) { |
bjorntukkertje | 11:aac90fca0290 | 201 | // 70 graden |
bjorntukkertje | 11:aac90fca0290 | 202 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 203 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 204 | MotorR3 = 0.65 ; |
bjorntukkertje | 11:aac90fca0290 | 205 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 206 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 207 | } |
bjorntukkertje | 11:aac90fca0290 | 208 | if (afstand > 0.25 && pos >= 211 && pos < 239) { |
bjorntukkertje | 11:aac90fca0290 | 209 | // 80 graden |
bjorntukkertje | 11:aac90fca0290 | 210 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 211 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 212 | MotorR3 = 0.6 ; |
bjorntukkertje | 11:aac90fca0290 | 213 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 214 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 215 | } |
bjorntukkertje | 11:aac90fca0290 | 216 | if (afstand > 0.25 && pos >= 239 && pos < 266) { |
bjorntukkertje | 11:aac90fca0290 | 217 | // 90 graden |
bjorntukkertje | 11:aac90fca0290 | 218 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 219 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 220 | MotorR3 = 0.55 ; |
bjorntukkertje | 11:aac90fca0290 | 221 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 222 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 223 | } |
bjorntukkertje | 11:aac90fca0290 | 224 | if (afstand > 0.25 && pos >= 266 && pos < 294) { |
bjorntukkertje | 11:aac90fca0290 | 225 | // 100 graden |
bjorntukkertje | 11:aac90fca0290 | 226 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 227 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 228 | MotorR3 = 0.5 ; |
bjorntukkertje | 11:aac90fca0290 | 229 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 230 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 231 | } |
bjorntukkertje | 11:aac90fca0290 | 232 | if (afstand > 0.25 && pos >= 294 && pos < 322) { |
bjorntukkertje | 11:aac90fca0290 | 233 | // 110 graden |
bjorntukkertje | 11:aac90fca0290 | 234 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 235 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 236 | MotorR3 = 0.45 ; |
bjorntukkertje | 11:aac90fca0290 | 237 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 238 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 239 | } |
bjorntukkertje | 11:aac90fca0290 | 240 | if (afstand > 0.25 && pos >= 322 && pos < 349) { |
bjorntukkertje | 11:aac90fca0290 | 241 | // 120 graden |
bjorntukkertje | 11:aac90fca0290 | 242 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 243 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 244 | MotorR3 = 0.4 ; |
bjorntukkertje | 11:aac90fca0290 | 245 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 246 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 247 | } |
bjorntukkertje | 11:aac90fca0290 | 248 | if (afstand > 0.25 && pos >= 349 && pos < 377) { |
bjorntukkertje | 11:aac90fca0290 | 249 | // 130 graden |
bjorntukkertje | 11:aac90fca0290 | 250 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 251 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 252 | MotorR3 = 0.35 ; |
bjorntukkertje | 11:aac90fca0290 | 253 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 254 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 255 | } |
bjorntukkertje | 11:aac90fca0290 | 256 | if (afstand > 0.25 && pos >= 377 && pos < 405) { |
bjorntukkertje | 11:aac90fca0290 | 257 | // 140 graden |
bjorntukkertje | 11:aac90fca0290 | 258 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 259 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 260 | MotorR3 = 0.3 ; |
bjorntukkertje | 11:aac90fca0290 | 261 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 262 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 263 | } |
bjorntukkertje | 11:aac90fca0290 | 264 | if (afstand > 0.25 && pos >= 405 && pos < 433) { |
bjorntukkertje | 11:aac90fca0290 | 265 | // 150 graden |
bjorntukkertje | 11:aac90fca0290 | 266 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 267 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 268 | MotorR3 = 0.25 ; |
bjorntukkertje | 11:aac90fca0290 | 269 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 270 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 271 | } |
bjorntukkertje | 11:aac90fca0290 | 272 | if (afstand > 0.25 && pos >= 433 && pos < 460) { |
bjorntukkertje | 11:aac90fca0290 | 273 | // 160 graden |
bjorntukkertje | 11:aac90fca0290 | 274 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 275 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 276 | MotorR3 = 0.2 ; |
bjorntukkertje | 11:aac90fca0290 | 277 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 278 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 279 | } |
bjorntukkertje | 11:aac90fca0290 | 280 | if (afstand > 0.25 && pos >= 460 && pos < 488) { |
bjorntukkertje | 11:aac90fca0290 | 281 | // 170 graden |
bjorntukkertje | 11:aac90fca0290 | 282 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 283 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 284 | MotorR3 = 0.15 ; |
bjorntukkertje | 11:aac90fca0290 | 285 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 286 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 287 | } |
bjorntukkertje | 11:aac90fca0290 | 288 | if (afstand > 0.25 && pos >= 488 && pos < 516) { |
bjorntukkertje | 11:aac90fca0290 | 289 | // 180 graden |
bjorntukkertje | 11:aac90fca0290 | 290 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 291 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 292 | MotorR3 = 0.1 ; |
bjorntukkertje | 11:aac90fca0290 | 293 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 294 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 295 | } |
bjorntukkertje | 11:aac90fca0290 | 296 | if (afstand > 0.25 && pos >= 516 && pos < 543) { |
bjorntukkertje | 11:aac90fca0290 | 297 | // 190 graden |
bjorntukkertje | 11:aac90fca0290 | 298 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 299 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 300 | MotorR3 = 0.15 ; |
bjorntukkertje | 11:aac90fca0290 | 301 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 302 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 303 | } |
bjorntukkertje | 11:aac90fca0290 | 304 | if (afstand > 0.25 && pos >= 543 && pos < 571) { |
bjorntukkertje | 11:aac90fca0290 | 305 | // 200 graden |
bjorntukkertje | 11:aac90fca0290 | 306 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 307 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 308 | MotorR3 = 0.2 ; |
bjorntukkertje | 11:aac90fca0290 | 309 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 310 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 311 | } |
bjorntukkertje | 11:aac90fca0290 | 312 | if (afstand > 0.25 && pos >= 571 && pos < 599) { |
bjorntukkertje | 11:aac90fca0290 | 313 | // 210 graden |
bjorntukkertje | 11:aac90fca0290 | 314 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 315 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 316 | MotorR3 = 0.25 ; |
bjorntukkertje | 11:aac90fca0290 | 317 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 318 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 319 | } |
bjorntukkertje | 11:aac90fca0290 | 320 | if (afstand > 0.25 && pos >= 599 && pos < 626) { |
bjorntukkertje | 11:aac90fca0290 | 321 | // 220 graden |
bjorntukkertje | 11:aac90fca0290 | 322 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 323 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 324 | MotorR3 = 0.30 ; |
bjorntukkertje | 11:aac90fca0290 | 325 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 326 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 327 | } |
bjorntukkertje | 11:aac90fca0290 | 328 | if (afstand > 0.25 && pos >= 626 && pos < 654) { |
bjorntukkertje | 11:aac90fca0290 | 329 | // 230 graden |
bjorntukkertje | 11:aac90fca0290 | 330 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 331 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 332 | MotorR3 = 0.35 ; |
bjorntukkertje | 11:aac90fca0290 | 333 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 334 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 335 | } |
bjorntukkertje | 11:aac90fca0290 | 336 | if (afstand > 0.25 && pos >= 654 && pos < 682) { |
bjorntukkertje | 11:aac90fca0290 | 337 | // 240 graden |
bjorntukkertje | 11:aac90fca0290 | 338 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 339 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 340 | MotorR3 = 0.40 ; |
bjorntukkertje | 11:aac90fca0290 | 341 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 342 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 343 | } |
bjorntukkertje | 11:aac90fca0290 | 344 | if (afstand > 0.25 && pos >= 682 && pos < 710) { |
bjorntukkertje | 11:aac90fca0290 | 345 | // 250 graden |
bjorntukkertje | 11:aac90fca0290 | 346 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 347 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 348 | MotorR3 = 0.45 ; |
bjorntukkertje | 11:aac90fca0290 | 349 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 350 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 351 | } |
bjorntukkertje | 11:aac90fca0290 | 352 | if (afstand > 0.25 && pos >= 710 && pos < 737) { |
bjorntukkertje | 11:aac90fca0290 | 353 | // 260 graden |
bjorntukkertje | 11:aac90fca0290 | 354 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 355 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 356 | MotorR3 = 0.50 ; |
bjorntukkertje | 11:aac90fca0290 | 357 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 358 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 359 | } |
bjorntukkertje | 11:aac90fca0290 | 360 | if (afstand > 0.25 && pos >= 737 && pos < 765) { |
bjorntukkertje | 11:aac90fca0290 | 361 | // 270 graden |
bjorntukkertje | 11:aac90fca0290 | 362 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 363 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 364 | MotorR3 = 0.55 ; |
bjorntukkertje | 11:aac90fca0290 | 365 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 366 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 367 | } |
bjorntukkertje | 11:aac90fca0290 | 368 | if (afstand > 0.25 && pos >= 765 && pos < 793) { |
bjorntukkertje | 11:aac90fca0290 | 369 | // 280 graden |
bjorntukkertje | 11:aac90fca0290 | 370 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 371 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 372 | MotorR3 = 0.60 ; |
bjorntukkertje | 11:aac90fca0290 | 373 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 374 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 375 | } |
bjorntukkertje | 11:aac90fca0290 | 376 | if (afstand > 0.25 && pos >= 793 && pos < 820) { |
bjorntukkertje | 11:aac90fca0290 | 377 | // 290 graden |
bjorntukkertje | 11:aac90fca0290 | 378 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 379 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 380 | MotorR3 = 0.65 ; |
bjorntukkertje | 11:aac90fca0290 | 381 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 382 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 383 | } |
bjorntukkertje | 11:aac90fca0290 | 384 | if (afstand > 0.25 && pos >= 820 && pos < 848) { |
bjorntukkertje | 11:aac90fca0290 | 385 | // 300 graden |
bjorntukkertje | 11:aac90fca0290 | 386 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 387 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 388 | MotorR3 = 0.70 ; |
bjorntukkertje | 11:aac90fca0290 | 389 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 390 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 391 | } |
bjorntukkertje | 11:aac90fca0290 | 392 | if (afstand > 0.25 && pos >= 848 && pos < 876) { |
bjorntukkertje | 11:aac90fca0290 | 393 | // 310 graden |
bjorntukkertje | 11:aac90fca0290 | 394 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 395 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 396 | MotorR3 = 0.75 ; |
bjorntukkertje | 11:aac90fca0290 | 397 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 398 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 399 | } |
bjorntukkertje | 11:aac90fca0290 | 400 | if (afstand > 0.25 && pos >= 876 && pos < 903) { |
bjorntukkertje | 11:aac90fca0290 | 401 | // 320 graden |
bjorntukkertje | 11:aac90fca0290 | 402 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 403 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 404 | MotorR3 = 0.80 ; |
bjorntukkertje | 11:aac90fca0290 | 405 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 406 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 407 | } |
bjorntukkertje | 11:aac90fca0290 | 408 | if (afstand > 0.25 && pos >= 903 && pos < 931) { |
bjorntukkertje | 11:aac90fca0290 | 409 | // 330 graden |
bjorntukkertje | 11:aac90fca0290 | 410 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 411 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 412 | MotorR3 = 0.85 ; |
bjorntukkertje | 11:aac90fca0290 | 413 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 414 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 415 | } |
bjorntukkertje | 11:aac90fca0290 | 416 | if (afstand > 0.25 && pos >= 931 && pos < 956) { |
bjorntukkertje | 11:aac90fca0290 | 417 | // 340 graden |
bjorntukkertje | 11:aac90fca0290 | 418 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 419 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 420 | MotorR3 = 0.90 ; |
bjorntukkertje | 11:aac90fca0290 | 421 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 422 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 423 | } |
bjorntukkertje | 11:aac90fca0290 | 424 | if (afstand > 0.25 && pos >= 956 && pos < 987) { |
bjorntukkertje | 11:aac90fca0290 | 425 | // 350 graden |
bjorntukkertje | 11:aac90fca0290 | 426 | MotorL1 = 1 ; |
bjorntukkertje | 11:aac90fca0290 | 427 | MotorL2 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 428 | MotorR3 = 0.95 ; |
bjorntukkertje | 11:aac90fca0290 | 429 | MotorR4 = 0 ; |
bjorntukkertje | 11:aac90fca0290 | 430 | wait_ms (1000); |
bjorntukkertje | 11:aac90fca0290 | 431 | } |
bjorntukkertje | 11:aac90fca0290 | 432 | } |
bjorntukkertje | 0:9756a10feb5f | 433 | } |