Alexander Lüthard / Mbed 2 deprecated Micromouse

Alle Prinf's auskommentiert

Dependencies:   mbed

Fork of Micromouse by Helvijs Kiselis

main.cpp@19:e2cb5b8a7a29, 2018-05-10 (annotated)

Committer:
luethale
Date:
Thu May 10 16:33:32 2018 +0000
Revision:
19:e2cb5b8a7a29
Parent:
18:3309329d5f42 
Alle Printf's auskommentiert

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 12:75d0291a9785 10 - Beschleunigung: Wann & Wo?
Helvis 12:75d0291a9785 11 - Zentrieren
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
luethale 19:e2cb5b8a7a29 31 AnalogIn lineSensor(PC_5);
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
Helvis 0:8491169be8fc 36 IRSensor irSensorL (distance2);
Helvis 0:8491169be8fc 37 IRSensor irSensorC (distance4);
Helvis 0:8491169be8fc 38 IRSensor irSensorR (distance1);
Helvis 16:c5b864804632 39
Helvis 16:c5b864804632 40 Timer t1;
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 12:75d0291a9785 84 void reverseToJunction(int junc, int &r, int route[]) {
Helvis 1:2b5f79285a3e 85
Helvis 12:75d0291a9785 86 do {
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 18:3309329d5f42 98 //motion.runTask(route,r,true);
luethale 19:e2cb5b8a7a29 99 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 100 route[r] = 0;
Helvis 1:2b5f79285a3e 101 r--;
Helvis 12:75d0291a9785 102 } while (junc < r );
Helvis 1:2b5f79285a3e 103 }
luethale 9:a01f90f88920 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 12:75d0291a9785 110 int route[200] = {0};
Helvis 12:75d0291a9785 111 int r = 0;
Helvis 1:2b5f79285a3e 112
Helvis 18:3309329d5f42 113 int junction[20] = {0};
Helvis 18:3309329d5f42 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 12:75d0291a9785 122 //infinite loop
luethale 9:a01f90f88920 123 while(1) {
Helvis 0:8491169be8fc 124
luethale 9:a01f90f88920 125 button.fall(&press); //User button einlesen
Helvis 0:8491169be8fc 126
Helvis 12:75d0291a9785 127 /**
Helvis 12:75d0291a9785 128 *
Helvis 12:75d0291a9785 129 * Search run
Helvis 12:75d0291a9785 130 *
Helvis 12:75d0291a9785 131 */
Helvis 1:2b5f79285a3e 132
luethale 9:a01f90f88920 133 while(start == 1 && Ziel == 0) {
Helvis 0:8491169be8fc 134
luethale 9:a01f90f88920 135 /**
luethale 9:a01f90f88920 136 * Entscheidung und Bewegung
luethale 9:a01f90f88920 137 */
Helvis 1:2b5f79285a3e 138
Helvis 1:2b5f79285a3e 139
luethale 9:a01f90f88920 140 float distanceL = irSensorL.readL();
luethale 9:a01f90f88920 141 float distanceC = irSensorC.readC();
luethale 9:a01f90f88920 142 float distanceR = irSensorR.readR();
Helvis 0:8491169be8fc 143
luethale 9:a01f90f88920 144 //Wall check
luethale 9:a01f90f88920 145 if (distanceL < thresholdL) lWall = 1;
luethale 9:a01f90f88920 146 else lWall = 0;
luethale 9:a01f90f88920 147 if (distanceC < thresholdC) cWall = 1;
luethale 9:a01f90f88920 148 else cWall = 0;
luethale 9:a01f90f88920 149 if (distanceR < thresholdR) rWall = 1;
luethale 9:a01f90f88920 150 else rWall = 0;
Helvis 0:8491169be8fc 151
luethale 9:a01f90f88920 152 //Junction Check
luethale 9:a01f90f88920 153 if ((lWall + cWall + rWall) < 2) {
Helvis 11:2960fc540616 154
Helvis 11:2960fc540616 155 if (junction[j] != r) {
Helvis 11:2960fc540616 156 if (junction[j] > 0) {
Helvis 11:2960fc540616 157 j += 1;
Helvis 11:2960fc540616 158 junction[j] = r;
Helvis 11:2960fc540616 159 }else{
Helvis 11:2960fc540616 160 junction[j] = r;
Helvis 11:2960fc540616 161 }
luethale 9:a01f90f88920 162 }
Helvis 8:8131269dc46e 163
luethale 19:e2cb5b8a7a29 164 // printf("Kreuzung: %d Schritt: %d\n", j, r);
luethale 9:a01f90f88920 165 }
Helvis 12:75d0291a9785 166 //No wall left
luethale 9:a01f90f88920 167 if (lWall == 0) {
Helvis 0:8491169be8fc 168
luethale 9:a01f90f88920 169 if (route[r] == LEFT) {
Helvis 1:2b5f79285a3e 170
luethale 9:a01f90f88920 171 route[r] = MOVE;
luethale 19:e2cb5b8a7a29 172 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 173 r++;
Helvis 1:2b5f79285a3e 174
luethale 9:a01f90f88920 175 motion.rotateL();
luethale 9:a01f90f88920 176 motion.scanMove();
luethale 14:2926932e26df 177 // motion.stop();
Helvis 1:2b5f79285a3e 178
luethale 9:a01f90f88920 179 }else if (route[r] == MOVE) {
Helvis 1:2b5f79285a3e 180
luethale 9:a01f90f88920 181 route[r] = RIGHT;
luethale 19:e2cb5b8a7a29 182 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 183 r++;
luethale 9:a01f90f88920 184 route[r] = MOVE;
luethale 19:e2cb5b8a7a29 185 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 186 r++;
Helvis 1:2b5f79285a3e 187
luethale 9:a01f90f88920 188 motion.rotateL();
luethale 9:a01f90f88920 189 motion.scanMove();
luethale 14:2926932e26df 190 //motion.stop();
Helvis 1:2b5f79285a3e 191
luethale 9:a01f90f88920 192 }else if (route[r] == RIGHT) {
luethale 9:a01f90f88920 193 // Kreuzung führt zu Sackgassen -> löschen
luethale 9:a01f90f88920 194 junction[j] = 0; /*Änderung alex*/
luethale 9:a01f90f88920 195 //if (j > 0) {j--;}
luethale 19:e2cb5b8a7a29 196 // printf("Kreuzung %d Schritt %d geloscht\n", j, r);
Helvis 11:2960fc540616 197 j -= 1;
Helvis 12:75d0291a9785 198 //reverseToJunction(junction[j], r, route);
Helvis 7:22392ed60534 199
luethale 9:a01f90f88920 200 /*for (int i = r; i > junction[j]; i--) {
luethale 9:a01f90f88920 201 r = i;
luethale 9:a01f90f88920 202 //invert rotation
luethale 9:a01f90f88920 203 if (route[r] == LEFT) {
luethale 9:a01f90f88920 204
luethale 9:a01f90f88920 205 route[r] = RIGHT;
Helvis 8:8131269dc46e 206
luethale 9:a01f90f88920 207 }else if (route[r] == RIGHT) {
Helvis 8:8131269dc46e 208
luethale 9:a01f90f88920 209 route[r] = LEFT;
luethale 9:a01f90f88920 210 }
luethale 9:a01f90f88920 211
luethale 9:a01f90f88920 212 motion.runTask(route[r]);
luethale 9:a01f90f88920 213 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 11:2960fc540616 214 route[r] = 0;
Helvis 11:2960fc540616 215
Helvis 11:2960fc540616 216 if ( r == junction[j] + 1) {
Helvis 11:2960fc540616 217 while (controller.getSpeedLeft() > 0.0f) {
Helvis 11:2960fc540616 218 motion.stop();
Helvis 11:2960fc540616 219 }
Helvis 11:2960fc540616 220 }
Helvis 11:2960fc540616 221
Helvis 8:8131269dc46e 222 }
Helvis 11:2960fc540616 223 motion.stop();
Helvis 11:2960fc540616 224
luethale 9:a01f90f88920 225 printf("Loop fertig\n");
luethale 9:a01f90f88920 226 r--;
Helvis 8:8131269dc46e 227 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 228 */
Helvis 8:8131269dc46e 229
Helvis 11:2960fc540616 230 do {
Helvis 7:22392ed60534 231
luethale 9:a01f90f88920 232 //invert rotation
luethale 9:a01f90f88920 233 if (route[r] == LEFT) {
Helvis 7:22392ed60534 234
luethale 9:a01f90f88920 235 route[r] = RIGHT;
luethale 9:a01f90f88920 236
luethale 9:a01f90f88920 237 }else if (route[r] == RIGHT) {
Helvis 7:22392ed60534 238
luethale 9:a01f90f88920 239 route[r] = LEFT;
luethale 9:a01f90f88920 240 }
luethale 9:a01f90f88920 241
Helvis 18:3309329d5f42 242 motion.runTask(route,r,true,junction[j]);
luethale 19:e2cb5b8a7a29 243 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 244 route[r] = 0;
luethale 9:a01f90f88920 245 r--;
Helvis 11:2960fc540616 246
Helvis 11:2960fc540616 247 } while (junction[j] < r ) ;
Helvis 11:2960fc540616 248 motion.stop();
Helvis 12:75d0291a9785 249 printf("Loop stop\n");
Helvis 11:2960fc540616 250
luethale 9:a01f90f88920 251 }else{
Helvis 11:2960fc540616 252
luethale 9:a01f90f88920 253 route[r] = LEFT;
luethale 19:e2cb5b8a7a29 254 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 255 r++;
luethale 9:a01f90f88920 256 route[r] = MOVE;
luethale 19:e2cb5b8a7a29 257 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 258 r++;
Helvis 1:2b5f79285a3e 259
Helvis 4:932eb2d29206 260
luethale 9:a01f90f88920 261 motion.rotateL();
luethale 9:a01f90f88920 262 motion.scanMove();
luethale 14:2926932e26df 263 // motion.stop();
luethale 9:a01f90f88920 264 }
Helvis 1:2b5f79285a3e 265 //No wall center
luethale 9:a01f90f88920 266 }else if (cWall == 0) {
luethale 9:a01f90f88920 267
luethale 9:a01f90f88920 268 if (route[r] == LEFT) {
luethale 9:a01f90f88920 269
luethale 9:a01f90f88920 270 route[r] = RIGHT;
luethale 19:e2cb5b8a7a29 271 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 272 r++;
luethale 9:a01f90f88920 273 route[r] = MOVE;
luethale 19:e2cb5b8a7a29 274 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 275 r++;
Helvis 0:8491169be8fc 276
luethale 9:a01f90f88920 277 motion.scanMove();
luethale 14:2926932e26df 278 // motion.stop();
luethale 9:a01f90f88920 279
luethale 9:a01f90f88920 280 }else if (route[r] == MOVE) {
luethale 9:a01f90f88920 281
luethale 9:a01f90f88920 282 junction[j] = 0; /*Änderung alex*/
luethale 9:a01f90f88920 283 //if (j > 0) {j--;}
luethale 19:e2cb5b8a7a29 284 // printf("Kreuzung %d Schritt %d geloscht\n", j, r);
Helvis 11:2960fc540616 285 j -= 1;
luethale 9:a01f90f88920 286 //reverseToJunction(junction[j], r, route);
Helvis 1:2b5f79285a3e 287
luethale 9:a01f90f88920 288 /*for (int i = r; i > junction[j]; i--) {
luethale 9:a01f90f88920 289 r = i;
luethale 9:a01f90f88920 290 //invert rotation
luethale 9:a01f90f88920 291 if (route[r] == LEFT) {
luethale 9:a01f90f88920 292
luethale 9:a01f90f88920 293 route[r] = RIGHT;
luethale 9:a01f90f88920 294
luethale 9:a01f90f88920 295 }else if (route[r] == RIGHT) {
luethale 9:a01f90f88920 296
luethale 9:a01f90f88920 297 route[r] = LEFT;
luethale 9:a01f90f88920 298 }
luethale 9:a01f90f88920 299
luethale 9:a01f90f88920 300 motion.runTask(route[r]);
luethale 9:a01f90f88920 301 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 11:2960fc540616 302 route[r] = 0;
Helvis 11:2960fc540616 303
Helvis 11:2960fc540616 304 if ( r == junction[j] + 1) {
Helvis 11:2960fc540616 305 while (controller.getSpeedLeft() > 0.0f) {
Helvis 11:2960fc540616 306 motion.stop();
Helvis 11:2960fc540616 307 }
Helvis 11:2960fc540616 308 }
Helvis 11:2960fc540616 309
luethale 9:a01f90f88920 310 }
Helvis 11:2960fc540616 311 motion.stop();
luethale 9:a01f90f88920 312 printf("Loop fertig\n");
luethale 9:a01f90f88920 313 r--;
luethale 9:a01f90f88920 314 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 315 */
luethale 9:a01f90f88920 316
Helvis 11:2960fc540616 317 do {
luethale 9:a01f90f88920 318
luethale 9:a01f90f88920 319 //invert rotation
luethale 9:a01f90f88920 320 if (route[r] == LEFT) {
luethale 9:a01f90f88920 321
luethale 9:a01f90f88920 322 route[r] = RIGHT;
luethale 9:a01f90f88920 323
luethale 9:a01f90f88920 324 }else if (route[r] == RIGHT) {
luethale 9:a01f90f88920 325
luethale 9:a01f90f88920 326 route[r] = LEFT;
luethale 9:a01f90f88920 327 }
luethale 9:a01f90f88920 328
Helvis 18:3309329d5f42 329 motion.runTask(route,r,true,junction[j]);
luethale 19:e2cb5b8a7a29 330 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 11:2960fc540616 331 route[r] = 0;
Helvis 11:2960fc540616 332 r--;
Helvis 11:2960fc540616 333
Helvis 11:2960fc540616 334 } while (junction[j] < r );
Helvis 11:2960fc540616 335 motion.stop();
luethale 19:e2cb5b8a7a29 336 // printf("Loop stop\n");
luethale 9:a01f90f88920 337
luethale 9:a01f90f88920 338 }else{
luethale 9:a01f90f88920 339
luethale 9:a01f90f88920 340 route[r] = MOVE;
luethale 19:e2cb5b8a7a29 341 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 342 r++;
luethale 9:a01f90f88920 343
luethale 9:a01f90f88920 344 motion.scanMove();
luethale 9:a01f90f88920 345 }
luethale 9:a01f90f88920 346 //No wall right
luethale 9:a01f90f88920 347 }else if (rWall == 0) {
luethale 9:a01f90f88920 348
Helvis 1:2b5f79285a3e 349 route[r] = RIGHT;
luethale 19:e2cb5b8a7a29 350 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 351 r++;
Helvis 7:22392ed60534 352 route[r] = MOVE;
luethale 19:e2cb5b8a7a29 353 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 7:22392ed60534 354 r++;
Helvis 0:8491169be8fc 355
luethale 9:a01f90f88920 356 motion.rotateR();
Helvis 1:2b5f79285a3e 357 motion.scanMove();
luethale 14:2926932e26df 358 // motion.stop();
luethale 9:a01f90f88920 359
luethale 9:a01f90f88920 360 //Dead end routine
luethale 9:a01f90f88920 361 }else if ((lWall + cWall + rWall) == 3) {
luethale 9:a01f90f88920 362
luethale 9:a01f90f88920 363 motion.rotate180();
luethale 19:e2cb5b8a7a29 364 // printf("Sackgasse Schritt: %d\n", r);
luethale 9:a01f90f88920 365 r--;
Helvis 16:c5b864804632 366 t1.reset();
Helvis 16:c5b864804632 367 t1.start();
Helvis 17:8a8758bfe3c5 368 while (t1 < 0.5f) {}
Helvis 16:c5b864804632 369 t1.stop();
luethale 9:a01f90f88920 370 //Return to last junction
Helvis 11:2960fc540616 371 while (junction[j] <= r ) {
luethale 9:a01f90f88920 372 //for (int i = r; i >= junction[j]; i--
Helvis 16:c5b864804632 373
Helvis 11:2960fc540616 374 if (junction[j] == r) {
luethale 9:a01f90f88920 375
luethale 9:a01f90f88920 376 switch (route[r]) {
luethale 9:a01f90f88920 377 case MOVE:
Helvis 18:3309329d5f42 378 motion.runTask(route,r,true, junction[j]);
luethale 19:e2cb5b8a7a29 379 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 380 r--;
luethale 9:a01f90f88920 381 break;
luethale 9:a01f90f88920 382 case LEFT:
luethale 9:a01f90f88920 383 r--;
luethale 9:a01f90f88920 384 break;
luethale 9:a01f90f88920 385 case RIGHT:
luethale 9:a01f90f88920 386 r--;
luethale 9:a01f90f88920 387 break;
luethale 9:a01f90f88920 388 }
Helvis 17:8a8758bfe3c5 389 motion.stop();
Helvis 17:8a8758bfe3c5 390 t1.reset();
Helvis 17:8a8758bfe3c5 391 t1.start();
Helvis 17:8a8758bfe3c5 392 while (t1 < 1.0f) {}
Helvis 17:8a8758bfe3c5 393 t1.stop();
Helvis 7:22392ed60534 394
luethale 9:a01f90f88920 395 }else{
luethale 9:a01f90f88920 396 //invert rotation
luethale 9:a01f90f88920 397 if (route[r] == LEFT) {
luethale 9:a01f90f88920 398
luethale 9:a01f90f88920 399 route[r] = RIGHT;
luethale 9:a01f90f88920 400
luethale 9:a01f90f88920 401 }else if (route[r] == RIGHT) {
Helvis 8:8131269dc46e 402
luethale 9:a01f90f88920 403 route[r] = LEFT;
luethale 9:a01f90f88920 404 }
luethale 9:a01f90f88920 405 //Run tasks in declining order
Helvis 18:3309329d5f42 406 motion.runTask(route,r,true, junction[j]);
luethale 19:e2cb5b8a7a29 407 // printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 408 route[r] = 0;
luethale 9:a01f90f88920 409 r--;
Helvis 8:8131269dc46e 410 }
luethale 9:a01f90f88920 411 }
luethale 9:a01f90f88920 412 r++;
luethale 9:a01f90f88920 413 }
luethale 9:a01f90f88920 414
luethale 9:a01f90f88920 415 if (motion.finish() == 1) {
luethale 9:a01f90f88920 416
luethale 9:a01f90f88920 417 /*while(r > 0) {
luethale 9:a01f90f88920 418 //Zum Start zurückfahren
luethale 9:a01f90f88920 419 if (route[r] == LEFT) {
Helvis 8:8131269dc46e 420
luethale 9:a01f90f88920 421 route[r] = RIGHT;
Helvis 7:22392ed60534 422
Helvis 7:22392ed60534 423 }else if (route[r] == RIGHT) {
luethale 9:a01f90f88920 424
Helvis 7:22392ed60534 425 route[r] = LEFT;
Helvis 7:22392ed60534 426 }
luethale 9:a01f90f88920 427
Helvis 7:22392ed60534 428 motion.runTask(route[r]);
luethale 9:a01f90f88920 429 r--;
luethale 9:a01f90f88920 430 }
Helvis 8:8131269dc46e 431
luethale 9:a01f90f88920 432 motion.runTask(route[r])
Helvis 7:22392ed60534 433 */
Helvis 2:f898adf2d817 434
luethale 9:a01f90f88920 435 Ziel = 1;
luethale 9:a01f90f88920 436 r = 0;
luethale 9:a01f90f88920 437 start = 0;
luethale 9:a01f90f88920 438 controller.counterReset();
luethale 9:a01f90f88920 439 }else{
luethale 9:a01f90f88920 440 Ziel = 0;
luethale 9:a01f90f88920 441 }
Helvis 0:8491169be8fc 442
Helvis 0:8491169be8fc 443
luethale 9:a01f90f88920 444 }
Helvis 0:8491169be8fc 445
Helvis 1:2b5f79285a3e 446 /**
Helvis 1:2b5f79285a3e 447 *
Helvis 1:2b5f79285a3e 448 * Speed run
Helvis 1:2b5f79285a3e 449 *
Helvis 1:2b5f79285a3e 450 */
Helvis 1:2b5f79285a3e 451
luethale 9:a01f90f88920 452 while ( start == 1 && Ziel == 1 ) {
Helvis 0:8491169be8fc 453
luethale 9:a01f90f88920 454 /*if (route[r] == route[r+1] && route[r] == route[r+2]) {
luethale 9:a01f90f88920 455 //Auf längere Strecke schneller fahren
luethale 9:a01f90f88920 456 route[r+1] = SPEED;
luethale 9:a01f90f88920 457 }*/
Helvis 7:22392ed60534 458
luethale 9:a01f90f88920 459 float distanceL = irSensorL.readL();
luethale 9:a01f90f88920 460 float distanceC = irSensorC.readC();
luethale 9:a01f90f88920 461 float distanceR = irSensorR.readR();
Helvis 7:22392ed60534 462
Helvis 18:3309329d5f42 463 motion.runTask(route,r,false,junction[j]);
luethale 9:a01f90f88920 464 r++;
Helvis 0:8491169be8fc 465
luethale 9:a01f90f88920 466 if (route[r] == 0) {
luethale 9:a01f90f88920 467 //Weg fertig
luethale 9:a01f90f88920 468 motion.stop();
luethale 9:a01f90f88920 469 start = 0;
luethale 9:a01f90f88920 470 }
luethale 9:a01f90f88920 471 }
Helvis 0:8491169be8fc 472
Helvis 1:2b5f79285a3e 473
Helvis 1:2b5f79285a3e 474
Helvis 1:2b5f79285a3e 475
luethale 9:a01f90f88920 476 }
luethale 9:a01f90f88920 477 }