Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

main.cpp@8:8131269dc46e, 2018-04-27 (annotated)

Committer:
Helvis
Date:
Fri Apr 27 16:28:34 2018 +0000
Revision:
8:8131269dc46e
Parent:
7:22392ed60534
Child:
9:a01f90f88920
v0.9.5c

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Helvis 4:932eb2d29206 1 /**
Helvis 1:2b5f79285a3e 2 * Micromouse PES2
Helvis 1:2b5f79285a3e 3 *
Helvis 1:2b5f79285a3e 4 * Suchfahrtalg. + Schnellfahrtalg.
Helvis 1:2b5f79285a3e 5 *
Helvis 1:2b5f79285a3e 6 */
Helvis 1:2b5f79285a3e 7
Helvis 1:2b5f79285a3e 8 /* todo:
Helvis 1:2b5f79285a3e 9
Helvis 1:2b5f79285a3e 10 - Regler gerade Fahrt
Helvis 1:2b5f79285a3e 11 - gespeicherten Weg fahren
Helvis 4:932eb2d29206 12
Helvis 4:932eb2d29206 13 Optimieren:
Helvis 1:2b5f79285a3e 14 - Beschleunigung
Helvis 7:22392ed60534 15 - Zentrieren
Helvis 1:2b5f79285a3e 16 - über längere Strecke schneller fahren
Helvis 4:932eb2d29206 17 - Abbiegen
Helvis 1:2b5f79285a3e 18
Helvis 1:2b5f79285a3e 19 */
Helvis 0:8491169be8fc 20 #include <mbed.h>
Helvis 0:8491169be8fc 21 #include "EncoderCounter.h"
Helvis 0:8491169be8fc 22 #include "Controller.h"
Helvis 0:8491169be8fc 23 #include "IRSensor.h"
Helvis 0:8491169be8fc 24 #include "Motion.h"
Helvis 0:8491169be8fc 25
Helvis 0:8491169be8fc 26 //User Button
Helvis 0:8491169be8fc 27 InterruptIn button(USER_BUTTON);
Helvis 0:8491169be8fc 28
Helvis 0:8491169be8fc 29 //Sensors:
Helvis 0:8491169be8fc 30
Helvis 0:8491169be8fc 31 AnalogIn lineSensor(PA_4);
Helvis 0:8491169be8fc 32 AnalogIn distance2(PC_3);
Helvis 0:8491169be8fc 33 AnalogIn distance4(PB_1);
Helvis 0:8491169be8fc 34 AnalogIn distance1(PC_2);
Helvis 0:8491169be8fc 35 AnalogIn distance3(PC_5);
Helvis 0:8491169be8fc 36
Helvis 0:8491169be8fc 37 IRSensor irSensorL (distance2);
Helvis 0:8491169be8fc 38 IRSensor irSensorC (distance4);
Helvis 0:8491169be8fc 39 IRSensor irSensorR (distance1);
Helvis 0:8491169be8fc 40 IRSensor irSensorB (distance3);
Helvis 0:8491169be8fc 41
Helvis 0:8491169be8fc 42 //Motors:
Helvis 0:8491169be8fc 43 DigitalOut myled(LED1);
Helvis 0:8491169be8fc 44
Helvis 0:8491169be8fc 45 DigitalOut enableMotorDriver(PB_2);
Helvis 0:8491169be8fc 46
Helvis 0:8491169be8fc 47 DigitalIn motorDriverFault(PB_14);
Helvis 0:8491169be8fc 48 DigitalIn motorDriverWarning(PB_15);
Helvis 0:8491169be8fc 49
Helvis 0:8491169be8fc 50 PwmOut pwmRight(PA_8);
Helvis 0:8491169be8fc 51 PwmOut pwmLeft(PA_10);
Helvis 0:8491169be8fc 52
Helvis 0:8491169be8fc 53 EncoderCounter counterRight(PB_6, PB_7);
Helvis 0:8491169be8fc 54 EncoderCounter counterLeft(PA_0, PA_1);
Helvis 0:8491169be8fc 55
Helvis 0:8491169be8fc 56
Helvis 0:8491169be8fc 57 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
Helvis 0:8491169be8fc 58
Helvis 1:2b5f79285a3e 59 Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC,
Helvis 1:2b5f79285a3e 60 irSensorR, lineSensor, enableMotorDriver);
Helvis 0:8491169be8fc 61
Helvis 0:8491169be8fc 62 //------------------------------------------------------------------------------
Helvis 0:8491169be8fc 63
Helvis 0:8491169be8fc 64 volatile int start = 0;
Helvis 1:2b5f79285a3e 65
Helvis 1:2b5f79285a3e 66 const int MOVE = 1;
Helvis 1:2b5f79285a3e 67 const int LEFT = 2;
Helvis 1:2b5f79285a3e 68 const int RIGHT = 3;
Helvis 1:2b5f79285a3e 69 const int SPEED = 4;
Helvis 1:2b5f79285a3e 70
Helvis 1:2b5f79285a3e 71 //Sensor tresholds [mm]
Helvis 3:076dd7ec7eb4 72 const float thresholdL = 80;
Helvis 3:076dd7ec7eb4 73 const float thresholdR = 80;
Helvis 1:2b5f79285a3e 74 const float thresholdC = 100;
Helvis 0:8491169be8fc 75
Helvis 0:8491169be8fc 76 //------------------------------------------------------------------------------
Helvis 0:8491169be8fc 77
Helvis 0:8491169be8fc 78 //User button toggle
Helvis 0:8491169be8fc 79 void press() {
Helvis 0:8491169be8fc 80 start = !start;
Helvis 0:8491169be8fc 81 }
Helvis 1:2b5f79285a3e 82
Helvis 1:2b5f79285a3e 83 //Return to last junction
Helvis 7:22392ed60534 84 void reverseToJunction(int junc, int& r, int route[]) {
Helvis 1:2b5f79285a3e 85
Helvis 1:2b5f79285a3e 86 while (junc < r ) {
Helvis 1:2b5f79285a3e 87
Helvis 1:2b5f79285a3e 88 //invert rotation
Helvis 1:2b5f79285a3e 89 if (route[r] == LEFT) {
Helvis 1:2b5f79285a3e 90
Helvis 1:2b5f79285a3e 91 route[r] = RIGHT;
Helvis 1:2b5f79285a3e 92
Helvis 1:2b5f79285a3e 93 }else if (route[r] == RIGHT) {
Helvis 1:2b5f79285a3e 94
Helvis 1:2b5f79285a3e 95 route[r] = LEFT;
Helvis 1:2b5f79285a3e 96 }
Helvis 1:2b5f79285a3e 97
Helvis 1:2b5f79285a3e 98 motion.runTask(route[r]);
Helvis 1:2b5f79285a3e 99 route[r] = 0;
Helvis 1:2b5f79285a3e 100 r--;
Helvis 1:2b5f79285a3e 101 }
Helvis 1:2b5f79285a3e 102 }
Helvis 0:8491169be8fc 103
Helvis 0:8491169be8fc 104
Helvis 0:8491169be8fc 105 //------------------------------------------------------------------------------
Helvis 0:8491169be8fc 106
Helvis 0:8491169be8fc 107 int main() {
Helvis 1:2b5f79285a3e 108 //Init
Helvis 0:8491169be8fc 109
Helvis 6:4868f789c223 110 int route[200] = {0};
Helvis 8:8131269dc46e 111 volatile int r = 0;
Helvis 1:2b5f79285a3e 112
Helvis 6:4868f789c223 113 int junction[20] = {0};
Helvis 1:2b5f79285a3e 114 int j = 0;
Helvis 0:8491169be8fc 115
Helvis 1:2b5f79285a3e 116 short lWall;
Helvis 1:2b5f79285a3e 117 short cWall;
Helvis 1:2b5f79285a3e 118 short rWall;
Helvis 0:8491169be8fc 119
Helvis 1:2b5f79285a3e 120 short Ziel = 0;
Helvis 1:2b5f79285a3e 121
Helvis 1:2b5f79285a3e 122 //loop
Helvis 0:8491169be8fc 123 while(1) {
Helvis 0:8491169be8fc 124
Helvis 0:8491169be8fc 125 button.fall(&press); //User button einlesen
Helvis 0:8491169be8fc 126
Helvis 1:2b5f79285a3e 127 /**
Helvis 1:2b5f79285a3e 128 *
Helvis 1:2b5f79285a3e 129 * Search run
Helvis 1:2b5f79285a3e 130 *
Helvis 1:2b5f79285a3e 131 */
Helvis 1:2b5f79285a3e 132
Helvis 1:2b5f79285a3e 133 while(start == 1 && Ziel == 0) {
Helvis 0:8491169be8fc 134
Helvis 1:2b5f79285a3e 135 /**
Helvis 1:2b5f79285a3e 136 * Entscheidung und Bewegung
Helvis 1:2b5f79285a3e 137 */
Helvis 1:2b5f79285a3e 138
Helvis 1:2b5f79285a3e 139
Helvis 0:8491169be8fc 140 float distanceL = irSensorL.readL();
Helvis 0:8491169be8fc 141 float distanceC = irSensorC.readC();
Helvis 0:8491169be8fc 142 float distanceR = irSensorR.readR();
Helvis 0:8491169be8fc 143
Helvis 1:2b5f79285a3e 144 //Wall check
Helvis 1:2b5f79285a3e 145 if (distanceL < thresholdL) lWall = 1;
Helvis 1:2b5f79285a3e 146 else lWall = 0;
Helvis 1:2b5f79285a3e 147 if (distanceC < thresholdC) cWall = 1;
Helvis 1:2b5f79285a3e 148 else cWall = 0;
Helvis 1:2b5f79285a3e 149 if (distanceR < thresholdR) rWall = 1;
Helvis 1:2b5f79285a3e 150 else rWall = 0;
Helvis 0:8491169be8fc 151
Helvis 1:2b5f79285a3e 152 //Junction Check
Helvis 1:2b5f79285a3e 153 if ((lWall + cWall + rWall) < 2) {
Helvis 0:8491169be8fc 154
Helvis 8:8131269dc46e 155
Helvis 8:8131269dc46e 156 if (junction[j-1] != r) {
Helvis 8:8131269dc46e 157 junction[j] = r;
Helvis 8:8131269dc46e 158 j++;
Helvis 8:8131269dc46e 159 }
Helvis 8:8131269dc46e 160
Helvis 8:8131269dc46e 161 printf("Kreuzung: %d Schritt: %d\n", j, r);
Helvis 0:8491169be8fc 162 }
Helvis 1:2b5f79285a3e 163 //No wall left
Helvis 1:2b5f79285a3e 164 if (lWall == 0) {
Helvis 0:8491169be8fc 165
Helvis 1:2b5f79285a3e 166 if (route[r] == LEFT) {
Helvis 1:2b5f79285a3e 167
Helvis 1:2b5f79285a3e 168 route[r] = MOVE;
Helvis 4:932eb2d29206 169 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 170 r++;
Helvis 1:2b5f79285a3e 171
Helvis 1:2b5f79285a3e 172 motion.rotateL();
Helvis 1:2b5f79285a3e 173 motion.scanMove();
Helvis 1:2b5f79285a3e 174 motion.stop();
Helvis 1:2b5f79285a3e 175
Helvis 1:2b5f79285a3e 176 }else if (route[r] == MOVE) {
Helvis 1:2b5f79285a3e 177
Helvis 1:2b5f79285a3e 178 route[r] = RIGHT;
Helvis 4:932eb2d29206 179 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 180 r++;
Helvis 1:2b5f79285a3e 181 route[r] = MOVE;
Helvis 4:932eb2d29206 182 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 183 r++;
Helvis 1:2b5f79285a3e 184
Helvis 1:2b5f79285a3e 185 motion.rotateL();
Helvis 1:2b5f79285a3e 186 motion.scanMove();
Helvis 1:2b5f79285a3e 187 motion.stop();
Helvis 1:2b5f79285a3e 188
Helvis 1:2b5f79285a3e 189 }else if (route[r] == RIGHT) {
Helvis 1:2b5f79285a3e 190 // Kreuzung führt zu Sackgassen -> löschen
Helvis 8:8131269dc46e 191 junction[j-1] = 0;
Helvis 4:932eb2d29206 192 //if (j > 0) {j--;}
Helvis 4:932eb2d29206 193 printf("Kreuzung %d Schritt %d geloscht\n", j, r);
Helvis 4:932eb2d29206 194 j--;
Helvis 7:22392ed60534 195 //reverseToJunction(junction[j], r, route);
Helvis 7:22392ed60534 196
Helvis 8:8131269dc46e 197 /*for (int i = r; i > junction[j]; i--) {
Helvis 8:8131269dc46e 198 r = i;
Helvis 8:8131269dc46e 199 //invert rotation
Helvis 8:8131269dc46e 200 if (route[r] == LEFT) {
Helvis 8:8131269dc46e 201
Helvis 8:8131269dc46e 202 route[r] = RIGHT;
Helvis 8:8131269dc46e 203
Helvis 8:8131269dc46e 204 }else if (route[r] == RIGHT) {
Helvis 8:8131269dc46e 205
Helvis 8:8131269dc46e 206 route[r] = LEFT;
Helvis 8:8131269dc46e 207 }
Helvis 8:8131269dc46e 208
Helvis 8:8131269dc46e 209 motion.runTask(route[r]);
Helvis 8:8131269dc46e 210 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 8:8131269dc46e 211 route[r] = 0;
Helvis 8:8131269dc46e 212 }
Helvis 8:8131269dc46e 213 printf("Loop fertig\n");
Helvis 8:8131269dc46e 214 r--;
Helvis 8:8131269dc46e 215 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 8:8131269dc46e 216 */
Helvis 8:8131269dc46e 217
Helvis 8:8131269dc46e 218 while (junction[j-1] < r ) {
Helvis 7:22392ed60534 219
Helvis 7:22392ed60534 220 //invert rotation
Helvis 7:22392ed60534 221 if (route[r] == LEFT) {
Helvis 7:22392ed60534 222
Helvis 7:22392ed60534 223 route[r] = RIGHT;
Helvis 7:22392ed60534 224
Helvis 7:22392ed60534 225 }else if (route[r] == RIGHT) {
Helvis 7:22392ed60534 226
Helvis 7:22392ed60534 227 route[r] = LEFT;
Helvis 7:22392ed60534 228 }
Helvis 7:22392ed60534 229
Helvis 7:22392ed60534 230 motion.runTask(route[r]);
Helvis 8:8131269dc46e 231 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 7:22392ed60534 232 route[r] = 0;
Helvis 7:22392ed60534 233 r--;
Helvis 8:8131269dc46e 234 }
Helvis 1:2b5f79285a3e 235 }else{
Helvis 1:2b5f79285a3e 236
Helvis 1:2b5f79285a3e 237 route[r] = LEFT;
Helvis 4:932eb2d29206 238 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 239 r++;
Helvis 1:2b5f79285a3e 240 route[r] = MOVE;
Helvis 4:932eb2d29206 241 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 242 r++;
Helvis 1:2b5f79285a3e 243
Helvis 4:932eb2d29206 244
Helvis 1:2b5f79285a3e 245 motion.rotateL();
Helvis 1:2b5f79285a3e 246 motion.scanMove();
Helvis 1:2b5f79285a3e 247 motion.stop();
Helvis 1:2b5f79285a3e 248 }
Helvis 1:2b5f79285a3e 249 //No wall center
Helvis 1:2b5f79285a3e 250 }else if (cWall == 0) {
Helvis 0:8491169be8fc 251
Helvis 1:2b5f79285a3e 252 if (route[r] == LEFT) {
Helvis 1:2b5f79285a3e 253
Helvis 1:2b5f79285a3e 254 route[r] = RIGHT;
Helvis 4:932eb2d29206 255 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 256 r++;
Helvis 7:22392ed60534 257 route[r] = MOVE;
Helvis 4:932eb2d29206 258 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 7:22392ed60534 259 r++;
Helvis 0:8491169be8fc 260
Helvis 1:2b5f79285a3e 261 motion.scanMove();
Helvis 6:4868f789c223 262 motion.stop();
Helvis 1:2b5f79285a3e 263
Helvis 1:2b5f79285a3e 264 }else if (route[r] == MOVE) {
Helvis 1:2b5f79285a3e 265
Helvis 8:8131269dc46e 266 junction[j-1] = 0;
Helvis 4:932eb2d29206 267 //if (j > 0) {j--;}
Helvis 4:932eb2d29206 268 printf("Kreuzung %d Schritt %d geloscht\n", j, r);
Helvis 4:932eb2d29206 269 j--;
Helvis 7:22392ed60534 270 //reverseToJunction(junction[j], r, route);
Helvis 7:22392ed60534 271
Helvis 8:8131269dc46e 272 /*for (int i = r; i > junction[j]; i--) {
Helvis 8:8131269dc46e 273 r = i;
Helvis 8:8131269dc46e 274 //invert rotation
Helvis 8:8131269dc46e 275 if (route[r] == LEFT) {
Helvis 8:8131269dc46e 276
Helvis 8:8131269dc46e 277 route[r] = RIGHT;
Helvis 8:8131269dc46e 278
Helvis 8:8131269dc46e 279 }else if (route[r] == RIGHT) {
Helvis 8:8131269dc46e 280
Helvis 8:8131269dc46e 281 route[r] = LEFT;
Helvis 8:8131269dc46e 282 }
Helvis 8:8131269dc46e 283
Helvis 8:8131269dc46e 284 motion.runTask(route[r]);
Helvis 8:8131269dc46e 285 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 8:8131269dc46e 286 route[r] = 0;
Helvis 8:8131269dc46e 287 }
Helvis 8:8131269dc46e 288 printf("Loop fertig\n");
Helvis 8:8131269dc46e 289 r--;
Helvis 8:8131269dc46e 290 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 8:8131269dc46e 291 */
Helvis 8:8131269dc46e 292
Helvis 8:8131269dc46e 293 while (junction[j-1] < r ) {
Helvis 7:22392ed60534 294
Helvis 7:22392ed60534 295 //invert rotation
Helvis 7:22392ed60534 296 if (route[r] == LEFT) {
Helvis 7:22392ed60534 297
Helvis 7:22392ed60534 298 route[r] = RIGHT;
Helvis 7:22392ed60534 299
Helvis 7:22392ed60534 300 }else if (route[r] == RIGHT) {
Helvis 7:22392ed60534 301
Helvis 7:22392ed60534 302 route[r] = LEFT;
Helvis 7:22392ed60534 303 }
Helvis 7:22392ed60534 304
Helvis 7:22392ed60534 305 motion.runTask(route[r]);
Helvis 8:8131269dc46e 306 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 7:22392ed60534 307 route[r] = 0;
Helvis 7:22392ed60534 308 r--;
Helvis 7:22392ed60534 309 }
Helvis 1:2b5f79285a3e 310
Helvis 1:2b5f79285a3e 311 }else{
Helvis 1:2b5f79285a3e 312
Helvis 1:2b5f79285a3e 313 route[r] = MOVE;
Helvis 4:932eb2d29206 314 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 315 r++;
Helvis 1:2b5f79285a3e 316
Helvis 1:2b5f79285a3e 317 motion.scanMove();
Helvis 1:2b5f79285a3e 318 }
Helvis 1:2b5f79285a3e 319 //No wall right
Helvis 1:2b5f79285a3e 320 }else if (rWall == 0) {
Helvis 1:2b5f79285a3e 321
Helvis 1:2b5f79285a3e 322 route[r] = RIGHT;
Helvis 4:932eb2d29206 323 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 324 r++;
Helvis 1:2b5f79285a3e 325 route[r] = MOVE;
Helvis 4:932eb2d29206 326 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 327 r++;
Helvis 1:2b5f79285a3e 328
Helvis 1:2b5f79285a3e 329 motion.rotateR();
Helvis 1:2b5f79285a3e 330 motion.scanMove();
Helvis 1:2b5f79285a3e 331 motion.stop();
Helvis 1:2b5f79285a3e 332
Helvis 1:2b5f79285a3e 333 //Dead end routine
Helvis 1:2b5f79285a3e 334 }else if ((lWall + cWall + rWall) == 3) {
Helvis 1:2b5f79285a3e 335
Helvis 1:2b5f79285a3e 336 motion.rotate180();
Helvis 4:932eb2d29206 337 printf("Sackgasse Schritt: %d\n", r);
Helvis 1:2b5f79285a3e 338 r--;
Helvis 1:2b5f79285a3e 339 //Return to last junction
Helvis 8:8131269dc46e 340 while (junction[j-1] <= r ) {
Helvis 8:8131269dc46e 341 //for (int i = r; i >= junction[j]; i--
Helvis 8:8131269dc46e 342
Helvis 8:8131269dc46e 343 if (junction[j-1] == r) {
Helvis 6:4868f789c223 344
Helvis 6:4868f789c223 345 switch (route[r]) {
Helvis 6:4868f789c223 346 case MOVE:
Helvis 6:4868f789c223 347 motion.runTask(route[r]);
Helvis 6:4868f789c223 348 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 6:4868f789c223 349 r--;
Helvis 8:8131269dc46e 350 break;
Helvis 6:4868f789c223 351 case LEFT:
Helvis 6:4868f789c223 352 r--;
Helvis 6:4868f789c223 353 break;
Helvis 6:4868f789c223 354 case RIGHT:
Helvis 6:4868f789c223 355 r--;
Helvis 6:4868f789c223 356 break;
Helvis 6:4868f789c223 357 }
Helvis 4:932eb2d29206 358
Helvis 4:932eb2d29206 359 }else{
Helvis 1:2b5f79285a3e 360 //invert rotation
Helvis 4:932eb2d29206 361 if (route[r] == LEFT) {
Helvis 4:932eb2d29206 362
Helvis 4:932eb2d29206 363 route[r] = RIGHT;
Helvis 4:932eb2d29206 364
Helvis 4:932eb2d29206 365 }else if (route[r] == RIGHT) {
Helvis 4:932eb2d29206 366
Helvis 4:932eb2d29206 367 route[r] = LEFT;
Helvis 6:4868f789c223 368 }
Helvis 6:4868f789c223 369 //Run tasks in declining order
Helvis 6:4868f789c223 370 motion.runTask(route[r]);
Helvis 6:4868f789c223 371 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 6:4868f789c223 372 route[r] = 0;
Helvis 6:4868f789c223 373 r--;
Helvis 4:932eb2d29206 374 }
Helvis 1:2b5f79285a3e 375 }
Helvis 4:932eb2d29206 376 r++;
Helvis 0:8491169be8fc 377 }
Helvis 1:2b5f79285a3e 378
Helvis 1:2b5f79285a3e 379 if (motion.finish() == 1) {
Helvis 2:f898adf2d817 380
Helvis 2:f898adf2d817 381 /*while(r > 0) {
Helvis 2:f898adf2d817 382 //Zum Start zurückfahren
Helvis 2:f898adf2d817 383 if (route[r] == LEFT) {
Helvis 2:f898adf2d817 384
Helvis 2:f898adf2d817 385 route[r] = RIGHT;
Helvis 2:f898adf2d817 386
Helvis 2:f898adf2d817 387 }else if (route[r] == RIGHT) {
Helvis 2:f898adf2d817 388
Helvis 2:f898adf2d817 389 route[r] = LEFT;
Helvis 2:f898adf2d817 390 }
Helvis 2:f898adf2d817 391
Helvis 2:f898adf2d817 392 motion.runTask(route[r]);
Helvis 2:f898adf2d817 393 r--;
Helvis 7:22392ed60534 394 }
Helvis 7:22392ed60534 395
Helvis 7:22392ed60534 396 motion.runTask(route[r])
Helvis 7:22392ed60534 397 */
Helvis 2:f898adf2d817 398
Helvis 1:2b5f79285a3e 399 Ziel = 1;
Helvis 1:2b5f79285a3e 400 r = 0;
Helvis 1:2b5f79285a3e 401 start = 0;
Helvis 7:22392ed60534 402 controller.counterReset();
Helvis 1:2b5f79285a3e 403 }else{
Helvis 1:2b5f79285a3e 404 Ziel = 0;
Helvis 0:8491169be8fc 405 }
Helvis 0:8491169be8fc 406
Helvis 0:8491169be8fc 407
Helvis 1:2b5f79285a3e 408 }
Helvis 0:8491169be8fc 409
Helvis 1:2b5f79285a3e 410 /**
Helvis 1:2b5f79285a3e 411 *
Helvis 1:2b5f79285a3e 412 * Speed run
Helvis 1:2b5f79285a3e 413 *
Helvis 1:2b5f79285a3e 414 */
Helvis 1:2b5f79285a3e 415
Helvis 1:2b5f79285a3e 416 while ( start == 1 && Ziel == 1 ) {
Helvis 0:8491169be8fc 417
Helvis 2:f898adf2d817 418 /*if (route[r] == route[r+1] && route[r] == route[r+2]) {
Helvis 1:2b5f79285a3e 419 //Auf längere Strecke schneller fahren
Helvis 1:2b5f79285a3e 420 route[r+1] = SPEED;
Helvis 2:f898adf2d817 421 }*/
Helvis 7:22392ed60534 422
Helvis 7:22392ed60534 423 float distanceL = irSensorL.readL();
Helvis 7:22392ed60534 424 float distanceC = irSensorC.readC();
Helvis 7:22392ed60534 425 float distanceR = irSensorR.readR();
Helvis 7:22392ed60534 426
Helvis 1:2b5f79285a3e 427 motion.runTask(route[r]);
Helvis 1:2b5f79285a3e 428 r++;
Helvis 0:8491169be8fc 429
Helvis 1:2b5f79285a3e 430 if (route[r] == 0) {
Helvis 1:2b5f79285a3e 431 //Weg fertig
Helvis 4:932eb2d29206 432 motion.stop();
Helvis 1:2b5f79285a3e 433 start = 0;
Helvis 1:2b5f79285a3e 434 }
Helvis 0:8491169be8fc 435 }
Helvis 0:8491169be8fc 436
Helvis 1:2b5f79285a3e 437
Helvis 1:2b5f79285a3e 438
Helvis 1:2b5f79285a3e 439
Helvis 0:8491169be8fc 440 }
Helvis 0:8491169be8fc 441 }
Helvis 0:8491169be8fc 442
Helvis 0:8491169be8fc 443
Helvis 0:8491169be8fc 444
Helvis 0:8491169be8fc 445