Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

main.cpp@36:99f60052c746, 2018-06-30 (annotated)

Committer:
luethale
Date:
Sat Jun 30 14:13:27 2018 +0000
Revision:
36:99f60052c746
Parent:
35:5a4e1a87b3da 
Nacht_der_Technik; Angepasste Werte:; Move & HalfeMove; Ev. Bechleunigung verbessern (aprupte Stops); Ev. Move & HalfeMove weiter anpassen

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
luethale 36:99f60052c746 57 const int debuging = 0;
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) {
Helvis 33:836ab2089565 162
Helvis 33:836ab2089565 163 int lastJunc = route[junction[j]];
luethale 9:a01f90f88920 164 // Kreuzung führt zu Sackgassen -> löschen
luethale 20:20573f55a5fd 165 junction[j] = 0;
Helvis 31:2c54f8304ef5 166 printf("Kreuzung %d Schritt %d geloscht\n", j, r);
Helvis 11:2960fc540616 167 j -= 1;
Helvis 32:e984b7959cb0 168
Helvis 32:e984b7959cb0 169 bool looping = false;
Helvis 8:8131269dc46e 170
Helvis 31:2c54f8304ef5 171 while (junction[j] <= r ) {
Helvis 7:22392ed60534 172
luethale 9:a01f90f88920 173 //invert rotation
Helvis 7:22392ed60534 174
Helvis 31:2c54f8304ef5 175 if (junction[j] == r) {
luethale 9:a01f90f88920 176
Helvis 31:2c54f8304ef5 177 switch (route[r]) {
Helvis 31:2c54f8304ef5 178 case MOVE:
Helvis 32:e984b7959cb0 179 if (looping == false) {
Helvis 31:2c54f8304ef5 180 motion.runTask(route, r, true, junction[j]);
Helvis 31:2c54f8304ef5 181 if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 32:e984b7959cb0 182 }
Helvis 31:2c54f8304ef5 183 r--;
Helvis 31:2c54f8304ef5 184 break;
Helvis 31:2c54f8304ef5 185 case LEFT:
Helvis 31:2c54f8304ef5 186 r--;
Helvis 31:2c54f8304ef5 187 break;
Helvis 31:2c54f8304ef5 188 case RIGHT:
Helvis 31:2c54f8304ef5 189 r--;
Helvis 31:2c54f8304ef5 190 break;
Helvis 31:2c54f8304ef5 191 }
Helvis 31:2c54f8304ef5 192 motion.stop();
Helvis 31:2c54f8304ef5 193 t1.reset();
Helvis 31:2c54f8304ef5 194 t1.start();
Helvis 31:2c54f8304ef5 195 while (t1 < 0.5f) {}
Helvis 31:2c54f8304ef5 196 t1.stop();
Helvis 31:2c54f8304ef5 197
Helvis 31:2c54f8304ef5 198 }else{
Helvis 31:2c54f8304ef5 199 if (route[r] == LEFT) {
Helvis 31:2c54f8304ef5 200
Helvis 31:2c54f8304ef5 201 route[r] = RIGHT;
Helvis 31:2c54f8304ef5 202
Helvis 31:2c54f8304ef5 203 }else if (route[r] == RIGHT) {
Helvis 31:2c54f8304ef5 204
Helvis 31:2c54f8304ef5 205 route[r] = LEFT;
Helvis 32:e984b7959cb0 206
Helvis 32:e984b7959cb0 207 }else if (junction[j] == r-1 && route[r] == MOVE) {
Helvis 32:e984b7959cb0 208
Helvis 32:e984b7959cb0 209 looping = true;
Helvis 32:e984b7959cb0 210
Helvis 33:836ab2089565 211 }else if (junction[j] == r-2 && route[r] == MOVE && route[junction[j]] != MOVE && lastJunc == MOVE) {
Helvis 32:e984b7959cb0 212
Helvis 32:e984b7959cb0 213 route[r] = 0;
Helvis 32:e984b7959cb0 214 r--;
Helvis 31:2c54f8304ef5 215 }
Helvis 7:22392ed60534 216
Helvis 31:2c54f8304ef5 217 motion.runTask(route, r, true, junction[j]);
Helvis 31:2c54f8304ef5 218 if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 31:2c54f8304ef5 219 route[r] = 0;
Helvis 31:2c54f8304ef5 220 r--;
Helvis 31:2c54f8304ef5 221 }
Helvis 31:2c54f8304ef5 222 }
Helvis 31:2c54f8304ef5 223 r++;
luethale 9:a01f90f88920 224
Helvis 11:2960fc540616 225 motion.stop();
Helvis 31:2c54f8304ef5 226 t1.reset();
Helvis 31:2c54f8304ef5 227 t1.start();
Helvis 31:2c54f8304ef5 228 while (t1 < 0.5f) {}
Helvis 31:2c54f8304ef5 229 t1.stop();
Helvis 31:2c54f8304ef5 230 if (debuging == 1) printf("Loop stop\n");
Helvis 11:2960fc540616 231
luethale 9:a01f90f88920 232 }else{
Helvis 11:2960fc540616 233
luethale 9:a01f90f88920 234 route[r] = LEFT;
Helvis 31:2c54f8304ef5 235 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 236 r++;
luethale 9:a01f90f88920 237 route[r] = MOVE;
Helvis 31:2c54f8304ef5 238 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 239 r++;
Helvis 1:2b5f79285a3e 240
Helvis 4:932eb2d29206 241
luethale 9:a01f90f88920 242 motion.rotateL();
luethale 9:a01f90f88920 243 motion.scanMove();
luethale 9:a01f90f88920 244 }
luethale 20:20573f55a5fd 245
luethale 20:20573f55a5fd 246 //No wall center
luethale 9:a01f90f88920 247 }else if (cWall == 0) {
luethale 9:a01f90f88920 248
luethale 9:a01f90f88920 249 if (route[r] == LEFT) {
luethale 9:a01f90f88920 250
luethale 9:a01f90f88920 251 route[r] = RIGHT;
Helvis 31:2c54f8304ef5 252 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 253 r++;
luethale 9:a01f90f88920 254 route[r] = MOVE;
Helvis 31:2c54f8304ef5 255 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 256 r++;
Helvis 0:8491169be8fc 257
luethale 9:a01f90f88920 258 motion.scanMove();
luethale 9:a01f90f88920 259
luethale 9:a01f90f88920 260 }else if (route[r] == MOVE) {
luethale 9:a01f90f88920 261
Helvis 33:836ab2089565 262 int lastJunc = route[junction[j]];
Helvis 33:836ab2089565 263
luethale 20:20573f55a5fd 264 junction[j] = 0;
Helvis 31:2c54f8304ef5 265 if (debuging == 1) printf("Kreuzung %d Schritt %d geloscht\n", j, r);
Helvis 11:2960fc540616 266 j -= 1;
Helvis 32:e984b7959cb0 267 bool looping = false;
luethale 9:a01f90f88920 268
Helvis 31:2c54f8304ef5 269 while (junction[j] <= r ) {
luethale 9:a01f90f88920 270
luethale 9:a01f90f88920 271 //invert rotation
luethale 9:a01f90f88920 272
Helvis 31:2c54f8304ef5 273 if (junction[j] == r) {
luethale 9:a01f90f88920 274
Helvis 31:2c54f8304ef5 275 switch (route[r]) {
Helvis 31:2c54f8304ef5 276 case MOVE:
Helvis 32:e984b7959cb0 277 if (looping == false) {
Helvis 31:2c54f8304ef5 278 motion.runTask(route, r, true, junction[j]);
Helvis 31:2c54f8304ef5 279 if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 32:e984b7959cb0 280 }
Helvis 31:2c54f8304ef5 281 r--;
Helvis 31:2c54f8304ef5 282 break;
Helvis 31:2c54f8304ef5 283 case LEFT:
Helvis 31:2c54f8304ef5 284 r--;
Helvis 31:2c54f8304ef5 285 break;
Helvis 31:2c54f8304ef5 286 case RIGHT:
Helvis 31:2c54f8304ef5 287 r--;
Helvis 31:2c54f8304ef5 288 break;
Helvis 31:2c54f8304ef5 289 }
Helvis 31:2c54f8304ef5 290 motion.stop();
Helvis 31:2c54f8304ef5 291 t1.reset();
Helvis 31:2c54f8304ef5 292 t1.start();
Helvis 31:2c54f8304ef5 293 while (t1 < 0.5f) {}
Helvis 31:2c54f8304ef5 294 t1.stop();
Helvis 31:2c54f8304ef5 295
Helvis 31:2c54f8304ef5 296 }else{
Helvis 31:2c54f8304ef5 297 if (route[r] == LEFT) {
Helvis 31:2c54f8304ef5 298
Helvis 31:2c54f8304ef5 299 route[r] = RIGHT;
Helvis 31:2c54f8304ef5 300
Helvis 31:2c54f8304ef5 301 }else if (route[r] == RIGHT) {
Helvis 31:2c54f8304ef5 302
Helvis 31:2c54f8304ef5 303 route[r] = LEFT;
Helvis 32:e984b7959cb0 304
Helvis 32:e984b7959cb0 305 }else if (junction[j] == r-1 && route[r] == MOVE) {
Helvis 32:e984b7959cb0 306
Helvis 32:e984b7959cb0 307 looping = true;
Helvis 32:e984b7959cb0 308
Helvis 33:836ab2089565 309 }else if (junction[j] == r-2 && route[r] == MOVE && route[junction[j]] != MOVE && lastJunc == MOVE) {
Helvis 32:e984b7959cb0 310
Helvis 32:e984b7959cb0 311 route[r] = 0;
Helvis 32:e984b7959cb0 312 r--;
Helvis 31:2c54f8304ef5 313 }
Helvis 11:2960fc540616 314
Helvis 31:2c54f8304ef5 315 motion.runTask(route, r, true, junction[j]);
Helvis 31:2c54f8304ef5 316 if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 31:2c54f8304ef5 317 route[r] = 0;
Helvis 31:2c54f8304ef5 318 r--;
Helvis 31:2c54f8304ef5 319 }
Helvis 21:41997651337a 320 }
Helvis 31:2c54f8304ef5 321 r++;
Helvis 11:2960fc540616 322 motion.stop();
Helvis 31:2c54f8304ef5 323 t1.reset();
Helvis 31:2c54f8304ef5 324 t1.start();
Helvis 31:2c54f8304ef5 325 while (t1 < 0.5f) {}
Helvis 31:2c54f8304ef5 326 t1.stop();
Helvis 31:2c54f8304ef5 327 if (debuging == 1) printf("Loop stop\n");
luethale 9:a01f90f88920 328
luethale 9:a01f90f88920 329 }else{
luethale 9:a01f90f88920 330
luethale 9:a01f90f88920 331 route[r] = MOVE;
Helvis 31:2c54f8304ef5 332 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 333 r++;
luethale 9:a01f90f88920 334
luethale 9:a01f90f88920 335 motion.scanMove();
luethale 9:a01f90f88920 336 }
luethale 20:20573f55a5fd 337
luethale 9:a01f90f88920 338 //No wall right
luethale 9:a01f90f88920 339 }else if (rWall == 0) {
luethale 9:a01f90f88920 340
Helvis 1:2b5f79285a3e 341 route[r] = RIGHT;
Helvis 31:2c54f8304ef5 342 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 343 r++;
Helvis 7:22392ed60534 344 route[r] = MOVE;
Helvis 31:2c54f8304ef5 345 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 7:22392ed60534 346 r++;
Helvis 0:8491169be8fc 347
luethale 9:a01f90f88920 348 motion.rotateR();
Helvis 1:2b5f79285a3e 349 motion.scanMove();
luethale 9:a01f90f88920 350
luethale 9:a01f90f88920 351 //Dead end routine
luethale 9:a01f90f88920 352 }else if ((lWall + cWall + rWall) == 3) {
luethale 9:a01f90f88920 353
luethale 9:a01f90f88920 354 motion.rotate180();
Helvis 31:2c54f8304ef5 355 if (debuging == 1) printf("Sackgasse Schritt: %d\n", r);
luethale 9:a01f90f88920 356 r--;
Helvis 16:c5b864804632 357 t1.reset();
Helvis 16:c5b864804632 358 t1.start();
Helvis 17:8a8758bfe3c5 359 while (t1 < 0.5f) {}
Helvis 16:c5b864804632 360 t1.stop();
luethale 9:a01f90f88920 361 //Return to last junction
Helvis 11:2960fc540616 362 while (junction[j] <= r ) {
Helvis 16:c5b864804632 363
Helvis 11:2960fc540616 364 if (junction[j] == r) {
luethale 9:a01f90f88920 365
luethale 9:a01f90f88920 366 switch (route[r]) {
luethale 9:a01f90f88920 367 case MOVE:
Helvis 21:41997651337a 368 motion.runTask(route, r, true, junction[j]);
Helvis 31:2c54f8304ef5 369 if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 370 r--;
luethale 9:a01f90f88920 371 break;
luethale 9:a01f90f88920 372 case LEFT:
luethale 9:a01f90f88920 373 r--;
luethale 9:a01f90f88920 374 break;
luethale 9:a01f90f88920 375 case RIGHT:
luethale 9:a01f90f88920 376 r--;
luethale 9:a01f90f88920 377 break;
luethale 9:a01f90f88920 378 }
Helvis 17:8a8758bfe3c5 379 motion.stop();
Helvis 17:8a8758bfe3c5 380 t1.reset();
Helvis 17:8a8758bfe3c5 381 t1.start();
Helvis 19:6cd6cc5c8b4c 382 while (t1 < 0.5f) {}
Helvis 17:8a8758bfe3c5 383 t1.stop();
Helvis 7:22392ed60534 384
luethale 9:a01f90f88920 385 }else{
luethale 9:a01f90f88920 386 //invert rotation
luethale 9:a01f90f88920 387 if (route[r] == LEFT) {
luethale 9:a01f90f88920 388
luethale 9:a01f90f88920 389 route[r] = RIGHT;
luethale 9:a01f90f88920 390
luethale 9:a01f90f88920 391 }else if (route[r] == RIGHT) {
Helvis 8:8131269dc46e 392
luethale 9:a01f90f88920 393 route[r] = LEFT;
luethale 9:a01f90f88920 394 }
luethale 9:a01f90f88920 395 //Run tasks in declining order
Helvis 21:41997651337a 396 motion.runTask(route, r, true, junction[j]);
Helvis 31:2c54f8304ef5 397 if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 398 route[r] = 0;
luethale 9:a01f90f88920 399 r--;
Helvis 8:8131269dc46e 400 }
luethale 9:a01f90f88920 401 }
luethale 9:a01f90f88920 402 r++;
luethale 9:a01f90f88920 403 }
luethale 9:a01f90f88920 404
luethale 20:20573f55a5fd 405 //finish line check
luethale 9:a01f90f88920 406 if (motion.finish() == 1) {
luethale 9:a01f90f88920 407
luethale 9:a01f90f88920 408 Ziel = 1;
Helvis 23:accd07ca2da7 409 r = 1;
Helvis 23:accd07ca2da7 410 //Convert rotations to smooth turns
Helvis 23:accd07ca2da7 411 while (route[r] != 0) {
Helvis 23:accd07ca2da7 412 //Convert pattern MOVE - ROTATE - MOVE
Helvis 23:accd07ca2da7 413 if (route[r-1] == MOVE && (route[r] == LEFT || route[r] == RIGHT) && route[r+1] == MOVE) {
Helvis 23:accd07ca2da7 414
Helvis 23:accd07ca2da7 415 route[r-1] = HALF_MOVE;
Helvis 23:accd07ca2da7 416 route[r+1] = HALF_MOVE;
Helvis 23:accd07ca2da7 417
Helvis 23:accd07ca2da7 418 if (route[r] == LEFT) route[r] = TURN_LEFT;
Helvis 23:accd07ca2da7 419 else route[r] = TURN_RIGHT;
Helvis 23:accd07ca2da7 420 //Convert consequent ROTATION to a smooth TURN
Helvis 23:accd07ca2da7 421 }else if (route[r-1] == HALF_MOVE && (route[r] == LEFT || route[r] == RIGHT) && route[r+1] == MOVE) {
Helvis 23:accd07ca2da7 422
Helvis 23:accd07ca2da7 423 route[r-1] = EMPTY;
Helvis 23:accd07ca2da7 424 route[r+1] = HALF_MOVE;
Helvis 23:accd07ca2da7 425
Helvis 23:accd07ca2da7 426 if (route[r] == LEFT) route[r] = TURN_LEFT;
Helvis 23:accd07ca2da7 427 else route[r] = TURN_RIGHT;
Helvis 23:accd07ca2da7 428
Helvis 23:accd07ca2da7 429 }
Helvis 23:accd07ca2da7 430
Helvis 23:accd07ca2da7 431 r++;
Helvis 23:accd07ca2da7 432 }
Helvis 23:accd07ca2da7 433 r = 0;
luethale 9:a01f90f88920 434 start = 0;
luethale 9:a01f90f88920 435 controller.counterReset();
Helvis 21:41997651337a 436 myled = 1;
luethale 9:a01f90f88920 437 }else{
luethale 9:a01f90f88920 438 Ziel = 0;
luethale 9:a01f90f88920 439 }
Helvis 0:8491169be8fc 440
Helvis 0:8491169be8fc 441
luethale 9:a01f90f88920 442 }
Helvis 0:8491169be8fc 443
Helvis 21:41997651337a 444 /*-------------------------------------------------------------------------*
Helvis 21:41997651337a 445 *
Helvis 21:41997651337a 446 * Speed run
Helvis 21:41997651337a 447 *
Helvis 21:41997651337a 448 *-------------------------------------------------------------------------*/
Helvis 1:2b5f79285a3e 449
luethale 9:a01f90f88920 450 while ( start == 1 && Ziel == 1 ) {
Helvis 0:8491169be8fc 451
Helvis 18:3309329d5f42 452 motion.runTask(route,r,false,junction[j]);
luethale 9:a01f90f88920 453 r++;
Helvis 0:8491169be8fc 454
luethale 9:a01f90f88920 455 if (route[r] == 0) {
luethale 9:a01f90f88920 456 //Weg fertig
luethale 9:a01f90f88920 457 motion.stop();
Helvis 21:41997651337a 458 start = 0;
Helvis 21:41997651337a 459 myled = 0;
luethale 9:a01f90f88920 460 }
luethale 9:a01f90f88920 461 }
Helvis 0:8491169be8fc 462
Helvis 1:2b5f79285a3e 463
Helvis 1:2b5f79285a3e 464
Helvis 1:2b5f79285a3e 465
luethale 9:a01f90f88920 466 }
luethale 9:a01f90f88920 467 }