Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

main.cpp@31:2c54f8304ef5, 2018-05-22 (annotated)

Committer:
Helvis
Date:
Tue May 22 23:35:53 2018 +0000
Revision:
31:2c54f8304ef5
Parent:
30:bdb8c92434a0
Child:
32:e984b7959cb0
v2.4 R?ckwertsfahren Kreuzung;

Who changed what in which revision?

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