Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

main.cpp@18:3309329d5f42, 2018-05-07 (annotated)

Committer:
Helvis
Date:
Mon May 07 19:52:54 2018 +0000
Revision:
18:3309329d5f42
Parent:
17:8a8758bfe3c5
Child:
19:6cd6cc5c8b4c
- komische Beschleunigung; - Anpassung

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