Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

main.cpp@9:a01f90f88920, 2018-04-28 (annotated)

Committer:
luethale
Date:
Sat Apr 28 12:29:20 2018 +0000
Revision:
9:a01f90f88920
Parent:
8:8131269dc46e
Child:
10:84534846e1f1
Child:
11:2960fc540616
Scan und Speicherfahrt funktioniert.

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 1:2b5f79285a3e 10 - Regler gerade Fahrt
Helvis 1:2b5f79285a3e 11 - gespeicherten Weg fahren
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 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
luethale 9:a01f90f88920 84 void reverseToJunction(int junc[], int j, int r, int route[]) {
Helvis 1:2b5f79285a3e 85
luethale 9:a01f90f88920 86 while (junc[j-1] < r ) {
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 1:2b5f79285a3e 98 motion.runTask(route[r]);
luethale 9:a01f90f88920 99 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 100 route[r] = 0;
Helvis 1:2b5f79285a3e 101 r--;
Helvis 1:2b5f79285a3e 102 }
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 6:4868f789c223 110 int route[200] = {0};
Helvis 8:8131269dc46e 111 volatile int r = 0;
Helvis 1:2b5f79285a3e 112
Helvis 6:4868f789c223 113 int junction[20] = {0};
Helvis 1:2b5f79285a3e 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 1:2b5f79285a3e 122 //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 1:2b5f79285a3e 127 /**
Helvis 1:2b5f79285a3e 128 *
Helvis 1:2b5f79285a3e 129 * Search run
Helvis 1:2b5f79285a3e 130 *
Helvis 1:2b5f79285a3e 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 0:8491169be8fc 154
Helvis 8:8131269dc46e 155
luethale 9:a01f90f88920 156 if (junction[j-1] != r) {
luethale 9:a01f90f88920 157 junction[j] = r;
luethale 9:a01f90f88920 158 j++;
luethale 9:a01f90f88920 159 }
Helvis 8:8131269dc46e 160
luethale 9:a01f90f88920 161 printf("Kreuzung: %d Schritt: %d\n", j, r);
luethale 9:a01f90f88920 162 }
Helvis 1:2b5f79285a3e 163 //No wall left
luethale 9:a01f90f88920 164 if (lWall == 0) {
Helvis 0:8491169be8fc 165
luethale 9:a01f90f88920 166 if (route[r] == LEFT) {
Helvis 1:2b5f79285a3e 167
luethale 9:a01f90f88920 168 route[r] = MOVE;
luethale 9:a01f90f88920 169 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 170 r++;
Helvis 1:2b5f79285a3e 171
luethale 9:a01f90f88920 172 motion.rotateL();
luethale 9:a01f90f88920 173 motion.scanMove();
luethale 9:a01f90f88920 174 motion.stop();
Helvis 1:2b5f79285a3e 175
luethale 9:a01f90f88920 176 }else if (route[r] == MOVE) {
Helvis 1:2b5f79285a3e 177
luethale 9:a01f90f88920 178 route[r] = RIGHT;
luethale 9:a01f90f88920 179 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 180 r++;
luethale 9:a01f90f88920 181 route[r] = MOVE;
luethale 9:a01f90f88920 182 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 183 r++;
Helvis 1:2b5f79285a3e 184
luethale 9:a01f90f88920 185 motion.rotateL();
luethale 9:a01f90f88920 186 motion.scanMove();
luethale 9:a01f90f88920 187 motion.stop();
Helvis 1:2b5f79285a3e 188
luethale 9:a01f90f88920 189 }else if (route[r] == RIGHT) {
luethale 9:a01f90f88920 190 // Kreuzung führt zu Sackgassen -> löschen
luethale 9:a01f90f88920 191 junction[j] = 0; /*Änderung alex*/
luethale 9:a01f90f88920 192 //if (j > 0) {j--;}
luethale 9:a01f90f88920 193 printf("Kreuzung %d Schritt %d geloscht\n", j, r);
luethale 9:a01f90f88920 194 j--;
luethale 9:a01f90f88920 195 //reverseToJunction(junction, j, r, route);
Helvis 7:22392ed60534 196
luethale 9:a01f90f88920 197 /*for (int i = r; i > junction[j]; i--) {
luethale 9:a01f90f88920 198 r = i;
luethale 9:a01f90f88920 199 //invert rotation
luethale 9:a01f90f88920 200 if (route[r] == LEFT) {
luethale 9:a01f90f88920 201
luethale 9:a01f90f88920 202 route[r] = RIGHT;
Helvis 8:8131269dc46e 203
luethale 9:a01f90f88920 204 }else if (route[r] == RIGHT) {
Helvis 8:8131269dc46e 205
luethale 9:a01f90f88920 206 route[r] = LEFT;
luethale 9:a01f90f88920 207 }
luethale 9:a01f90f88920 208
luethale 9:a01f90f88920 209 motion.runTask(route[r]);
luethale 9:a01f90f88920 210 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 211 route[r] = 0;
Helvis 8:8131269dc46e 212 }
luethale 9:a01f90f88920 213 printf("Loop fertig\n");
luethale 9:a01f90f88920 214 r--;
Helvis 8:8131269dc46e 215 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 216 */
Helvis 8:8131269dc46e 217
luethale 9:a01f90f88920 218 while (junction[j-1] < r ) {
Helvis 7:22392ed60534 219
luethale 9:a01f90f88920 220 //invert rotation
luethale 9:a01f90f88920 221 if (route[r] == LEFT) {
Helvis 7:22392ed60534 222
luethale 9:a01f90f88920 223 route[r] = RIGHT;
luethale 9:a01f90f88920 224
luethale 9:a01f90f88920 225 }else if (route[r] == RIGHT) {
Helvis 7:22392ed60534 226
luethale 9:a01f90f88920 227 route[r] = LEFT;
luethale 9:a01f90f88920 228 }
luethale 9:a01f90f88920 229
luethale 9:a01f90f88920 230 motion.runTask(route[r]);
luethale 9:a01f90f88920 231 printf("Schritt: %d, Befehl: %d\n", r, route[r]); //At work!!!!!
luethale 9:a01f90f88920 232 route[r] = 0;
luethale 9:a01f90f88920 233 r--;
Helvis 7:22392ed60534 234 }
luethale 9:a01f90f88920 235 printf("loop end");
luethale 9:a01f90f88920 236 }else{
luethale 9:a01f90f88920 237 printf("else");
luethale 9:a01f90f88920 238 route[r] = LEFT;
Helvis 8:8131269dc46e 239 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 240 r++;
luethale 9:a01f90f88920 241 route[r] = MOVE;
luethale 9:a01f90f88920 242 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 243 r++;
Helvis 1:2b5f79285a3e 244
Helvis 4:932eb2d29206 245
luethale 9:a01f90f88920 246 motion.rotateL();
luethale 9:a01f90f88920 247 motion.scanMove();
luethale 9:a01f90f88920 248 motion.stop();
luethale 9:a01f90f88920 249 }
Helvis 1:2b5f79285a3e 250 //No wall center
luethale 9:a01f90f88920 251 }else if (cWall == 0) {
luethale 9:a01f90f88920 252
luethale 9:a01f90f88920 253 if (route[r] == LEFT) {
luethale 9:a01f90f88920 254
luethale 9:a01f90f88920 255 route[r] = RIGHT;
luethale 9:a01f90f88920 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 0:8491169be8fc 261
luethale 9:a01f90f88920 262 motion.scanMove();
luethale 9:a01f90f88920 263 motion.stop();
luethale 9:a01f90f88920 264
luethale 9:a01f90f88920 265 }else if (route[r] == MOVE) {
luethale 9:a01f90f88920 266
luethale 9:a01f90f88920 267 junction[j] = 0; /*Änderung alex*/
luethale 9:a01f90f88920 268 //if (j > 0) {j--;}
luethale 9:a01f90f88920 269 printf("Kreuzung %d Schritt %d geloscht\n", j, r);
luethale 9:a01f90f88920 270 j--;
luethale 9:a01f90f88920 271 //reverseToJunction(junction[j], r, route);
Helvis 1:2b5f79285a3e 272
luethale 9:a01f90f88920 273 /*for (int i = r; i > junction[j]; i--) {
luethale 9:a01f90f88920 274 r = i;
luethale 9:a01f90f88920 275 //invert rotation
luethale 9:a01f90f88920 276 if (route[r] == LEFT) {
luethale 9:a01f90f88920 277
luethale 9:a01f90f88920 278 route[r] = RIGHT;
luethale 9:a01f90f88920 279
luethale 9:a01f90f88920 280 }else if (route[r] == RIGHT) {
luethale 9:a01f90f88920 281
luethale 9:a01f90f88920 282 route[r] = LEFT;
luethale 9:a01f90f88920 283 }
luethale 9:a01f90f88920 284
luethale 9:a01f90f88920 285 motion.runTask(route[r]);
luethale 9:a01f90f88920 286 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 287 route[r] = 0;
luethale 9:a01f90f88920 288 }
luethale 9:a01f90f88920 289 printf("Loop fertig\n");
luethale 9:a01f90f88920 290 r--;
luethale 9:a01f90f88920 291 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 292 */
luethale 9:a01f90f88920 293
luethale 9:a01f90f88920 294 while (junction[j-1] < r ) {
luethale 9:a01f90f88920 295
luethale 9:a01f90f88920 296 //invert rotation
luethale 9:a01f90f88920 297 if (route[r] == LEFT) {
luethale 9:a01f90f88920 298
luethale 9:a01f90f88920 299 route[r] = RIGHT;
luethale 9:a01f90f88920 300
luethale 9:a01f90f88920 301 }else if (route[r] == RIGHT) {
luethale 9:a01f90f88920 302
luethale 9:a01f90f88920 303 route[r] = LEFT;
luethale 9:a01f90f88920 304 }
luethale 9:a01f90f88920 305
luethale 9:a01f90f88920 306 motion.runTask(route[r]);
luethale 9:a01f90f88920 307 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 308 route[r] = 0;
luethale 9:a01f90f88920 309 r--;
luethale 9:a01f90f88920 310 }
luethale 9:a01f90f88920 311
luethale 9:a01f90f88920 312 }else{
luethale 9:a01f90f88920 313
luethale 9:a01f90f88920 314 route[r] = MOVE;
luethale 9:a01f90f88920 315 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 316 r++;
luethale 9:a01f90f88920 317
luethale 9:a01f90f88920 318 motion.scanMove();
luethale 9:a01f90f88920 319 }
luethale 9:a01f90f88920 320 //No wall right
luethale 9:a01f90f88920 321 }else if (rWall == 0) {
luethale 9:a01f90f88920 322
Helvis 1:2b5f79285a3e 323 route[r] = RIGHT;
Helvis 4:932eb2d29206 324 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 1:2b5f79285a3e 325 r++;
Helvis 7:22392ed60534 326 route[r] = MOVE;
Helvis 4:932eb2d29206 327 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
Helvis 7:22392ed60534 328 r++;
Helvis 0:8491169be8fc 329
luethale 9:a01f90f88920 330 motion.rotateR();
Helvis 1:2b5f79285a3e 331 motion.scanMove();
Helvis 6:4868f789c223 332 motion.stop();
luethale 9:a01f90f88920 333
luethale 9:a01f90f88920 334 //Dead end routine
luethale 9:a01f90f88920 335 }else if ((lWall + cWall + rWall) == 3) {
luethale 9:a01f90f88920 336
luethale 9:a01f90f88920 337 motion.rotate180();
luethale 9:a01f90f88920 338 printf("Sackgasse Schritt: %d\n", r);
luethale 9:a01f90f88920 339 r--;
luethale 9:a01f90f88920 340 //Return to last junction
luethale 9:a01f90f88920 341 while (junction[j-1] <= r ) {
luethale 9:a01f90f88920 342 //for (int i = r; i >= junction[j]; i--
luethale 9:a01f90f88920 343
luethale 9:a01f90f88920 344 if (junction[j-1] == r) {
luethale 9:a01f90f88920 345
luethale 9:a01f90f88920 346 switch (route[r]) {
luethale 9:a01f90f88920 347 case MOVE:
luethale 9:a01f90f88920 348 motion.runTask(route[r]);
luethale 9:a01f90f88920 349 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 350 r--;
luethale 9:a01f90f88920 351 break;
luethale 9:a01f90f88920 352 case LEFT:
luethale 9:a01f90f88920 353 r--;
luethale 9:a01f90f88920 354 break;
luethale 9:a01f90f88920 355 case RIGHT:
luethale 9:a01f90f88920 356 r--;
luethale 9:a01f90f88920 357 break;
luethale 9:a01f90f88920 358 }
Helvis 7:22392ed60534 359
luethale 9:a01f90f88920 360 }else{
luethale 9:a01f90f88920 361 //invert rotation
luethale 9:a01f90f88920 362 if (route[r] == LEFT) {
luethale 9:a01f90f88920 363
luethale 9:a01f90f88920 364 route[r] = RIGHT;
luethale 9:a01f90f88920 365
luethale 9:a01f90f88920 366 }else if (route[r] == RIGHT) {
Helvis 8:8131269dc46e 367
luethale 9:a01f90f88920 368 route[r] = LEFT;
luethale 9:a01f90f88920 369 }
luethale 9:a01f90f88920 370 //Run tasks in declining order
luethale 9:a01f90f88920 371 motion.runTask(route[r]);
luethale 9:a01f90f88920 372 printf("Schritt: %d, Befehl: %d\n", r, route[r]);
luethale 9:a01f90f88920 373 route[r] = 0;
luethale 9:a01f90f88920 374 r--;
Helvis 8:8131269dc46e 375 }
luethale 9:a01f90f88920 376 }
luethale 9:a01f90f88920 377 r++;
luethale 9:a01f90f88920 378 }
luethale 9:a01f90f88920 379
luethale 9:a01f90f88920 380 if (motion.finish() == 1) {
luethale 9:a01f90f88920 381
luethale 9:a01f90f88920 382 /*while(r > 0) {
luethale 9:a01f90f88920 383 //Zum Start zurückfahren
luethale 9:a01f90f88920 384 if (route[r] == LEFT) {
Helvis 8:8131269dc46e 385
luethale 9:a01f90f88920 386 route[r] = RIGHT;
Helvis 7:22392ed60534 387
Helvis 7:22392ed60534 388 }else if (route[r] == RIGHT) {
luethale 9:a01f90f88920 389
Helvis 7:22392ed60534 390 route[r] = LEFT;
Helvis 7:22392ed60534 391 }
luethale 9:a01f90f88920 392
Helvis 7:22392ed60534 393 motion.runTask(route[r]);
luethale 9:a01f90f88920 394 r--;
luethale 9:a01f90f88920 395 }
Helvis 8:8131269dc46e 396
luethale 9:a01f90f88920 397 motion.runTask(route[r])
Helvis 7:22392ed60534 398 */
Helvis 2:f898adf2d817 399
luethale 9:a01f90f88920 400 Ziel = 1;
luethale 9:a01f90f88920 401 r = 0;
luethale 9:a01f90f88920 402 start = 0;
luethale 9:a01f90f88920 403 controller.counterReset();
luethale 9:a01f90f88920 404 }else{
luethale 9:a01f90f88920 405 Ziel = 0;
luethale 9:a01f90f88920 406 }
Helvis 0:8491169be8fc 407
Helvis 0:8491169be8fc 408
luethale 9:a01f90f88920 409 }
Helvis 0:8491169be8fc 410
Helvis 1:2b5f79285a3e 411 /**
Helvis 1:2b5f79285a3e 412 *
Helvis 1:2b5f79285a3e 413 * Speed run
Helvis 1:2b5f79285a3e 414 *
Helvis 1:2b5f79285a3e 415 */
Helvis 1:2b5f79285a3e 416
luethale 9:a01f90f88920 417 while ( start == 1 && Ziel == 1 ) {
Helvis 0:8491169be8fc 418
luethale 9:a01f90f88920 419 /*if (route[r] == route[r+1] && route[r] == route[r+2]) {
luethale 9:a01f90f88920 420 //Auf längere Strecke schneller fahren
luethale 9:a01f90f88920 421 route[r+1] = SPEED;
luethale 9:a01f90f88920 422 }*/
Helvis 7:22392ed60534 423
luethale 9:a01f90f88920 424 float distanceL = irSensorL.readL();
luethale 9:a01f90f88920 425 float distanceC = irSensorC.readC();
luethale 9:a01f90f88920 426 float distanceR = irSensorR.readR();
Helvis 7:22392ed60534 427
luethale 9:a01f90f88920 428 motion.runTask(route[r]);
luethale 9:a01f90f88920 429 r++;
Helvis 0:8491169be8fc 430
luethale 9:a01f90f88920 431 if (route[r] == 0) {
luethale 9:a01f90f88920 432 //Weg fertig
luethale 9:a01f90f88920 433 motion.stop();
luethale 9:a01f90f88920 434 start = 0;
luethale 9:a01f90f88920 435 }
luethale 9:a01f90f88920 436 }
Helvis 0:8491169be8fc 437
Helvis 1:2b5f79285a3e 438
Helvis 1:2b5f79285a3e 439
Helvis 1:2b5f79285a3e 440
luethale 9:a01f90f88920 441 }
luethale 9:a01f90f88920 442 }