Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

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

Committer:
Helvis
Date:
Thu May 10 18:10:15 2018 +0000
Revision:
19:6cd6cc5c8b4c
Parent:
18:3309329d5f42
Child:
20:20573f55a5fd
v.1.0 +printf auskommnetiert; -unerwartete Beschleunigung (tragetSpeed)

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