Helvijs Kiselis
Algorithmus
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /*----------------------------------------------------------------------------* 00002 * Micromouse PES2 00003 * 00004 * PES2 2018, TEAM 3 00005 * 00006 * Code entwickelt von: 00007 * 00008 * 00009 *-----------------------------------------------------------------------------*/ 00010 00011 #include <mbed.h> 00012 #include "EncoderCounter.h" 00013 #include "Controller.h" 00014 #include "IRSensor.h" 00015 #include "Motion.h" 00016 00017 //User Button 00018 InterruptIn button(USER_BUTTON); 00019 00020 //Sensors: 00021 00022 AnalogIn lineSensor(PC_5); 00023 AnalogIn distance2(PC_3); 00024 AnalogIn distance4(PB_1); 00025 AnalogIn distance1(PC_2); 00026 00027 IRSensor irSensorL (distance2); 00028 IRSensor irSensorC (distance4); 00029 IRSensor irSensorR (distance1); 00030 00031 Timer t1; 00032 00033 //Motors: 00034 DigitalOut myled(LED1); 00035 00036 DigitalOut enableMotorDriver(PB_2); 00037 00038 DigitalIn motorDriverFault(PB_14); 00039 DigitalIn motorDriverWarning(PB_15); 00040 00041 PwmOut pwmRight(PA_8); 00042 PwmOut pwmLeft(PA_10); 00043 00044 EncoderCounter counterRight(PB_6, PB_7); 00045 EncoderCounter counterLeft(PA_0, PA_1); 00046 00047 00048 Controller controller(pwmLeft, pwmRight, counterLeft, counterRight); 00049 00050 Motion motion(controller, counterLeft, counterRight, irSensorL, irSensorC, 00051 irSensorR, lineSensor, enableMotorDriver); 00052 00053 //------------------------------------------------------------------------------ 00054 00055 volatile int start = 0; 00056 00057 const int debuging = 0; 00058 00059 const int MOVE = 1; 00060 const int LEFT = 2; 00061 const int RIGHT = 3; 00062 const int HALF_MOVE = 4; 00063 const int TURN_LEFT = 5; 00064 const int TURN_RIGHT = 6; 00065 const int EMPTY = 7; 00066 00067 //Sensor tresholds [mm] 00068 const float thresholdL = 70; 00069 const float thresholdR = 70; 00070 const float thresholdC = 80; 00071 00072 //------------------------------------------------------------------------------ 00073 00074 //User button toggle 00075 void press() { 00076 start = !start; 00077 } 00078 00079 //------------------------------------------------------------------------------ 00080 00081 int main() { 00082 //Init 00083 00084 int route[200] = {0}; 00085 int r = 0; 00086 00087 int junction[20] = {0}; 00088 int j = 0; 00089 00090 short lWall; 00091 short cWall; 00092 short rWall; 00093 00094 short Ziel = 0; 00095 00096 //infinite loop 00097 while(1) { 00098 00099 button.fall(&press); //User button einlesen 00100 00101 /*-----------------------------------------------------------------------------* 00102 * 00103 * Search run 00104 * 00105 *-----------------------------------------------------------------------------*/ 00106 00107 while(start == 1 && Ziel == 0) { 00108 00109 00110 float distanceL = irSensorL.readL(); 00111 float distanceC = irSensorC.readC(); 00112 float distanceR = irSensorR.readR(); 00113 00114 //Wall check 00115 if (distanceL < thresholdL) lWall = 1; 00116 else lWall = 0; 00117 if (distanceC < thresholdC) cWall = 1; 00118 else cWall = 0; 00119 if (distanceR < thresholdR) rWall = 1; 00120 else rWall = 0; 00121 00122 //Junction Check 00123 if ((lWall + cWall + rWall) < 2) { 00124 00125 if (junction[j] != r) { 00126 if (junction[j] > 0) { 00127 j += 1; 00128 junction[j] = r; 00129 }else{ 00130 junction[j] = r; 00131 } 00132 } 00133 00134 if (debuging == 1) printf("Kreuzung: %d Schritt: %d\n", j, r); 00135 } 00136 00137 //No wall left 00138 if (lWall == 0) { 00139 00140 if (route[r] == LEFT) { 00141 00142 route[r] = MOVE; 00143 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); 00144 r++; 00145 00146 motion.rotateL(); 00147 motion.scanMove(); 00148 00149 }else if (route[r] == MOVE) { 00150 00151 route[r] = RIGHT; 00152 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); 00153 r++; 00154 route[r] = MOVE; 00155 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); 00156 r++; 00157 00158 motion.rotateL(); 00159 motion.scanMove(); 00160 00161 }else if (route[r] == RIGHT) { 00162 00163 int lastJunc = route[junction[j]]; 00164 // Kreuzung führt zu Sackgassen -> löschen 00165 junction[j] = 0; 00166 printf("Kreuzung %d Schritt %d geloscht\n", j, r); 00167 j -= 1; 00168 00169 bool looping = false; 00170 00171 while (junction[j] <= r ) { 00172 00173 //invert rotation 00174 00175 if (junction[j] == r) { 00176 00177 switch (route[r]) { 00178 case MOVE: 00179 if (looping == false) { 00180 motion.runTask(route, r, true, junction[j]); 00181 if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]); 00182 } 00183 r--; 00184 break; 00185 case LEFT: 00186 r--; 00187 break; 00188 case RIGHT: 00189 r--; 00190 break; 00191 } 00192 motion.stop(); 00193 t1.reset(); 00194 t1.start(); 00195 while (t1 < 0.5f) {} 00196 t1.stop(); 00197 00198 }else{ 00199 if (route[r] == LEFT) { 00200 00201 route[r] = RIGHT; 00202 00203 }else if (route[r] == RIGHT) { 00204 00205 route[r] = LEFT; 00206 00207 }else if (junction[j] == r-1 && route[r] == MOVE) { 00208 00209 looping = true; 00210 00211 }else if (junction[j] == r-2 && route[r] == MOVE && route[junction[j]] != MOVE && lastJunc == MOVE) { 00212 00213 route[r] = 0; 00214 r--; 00215 } 00216 00217 motion.runTask(route, r, true, junction[j]); 00218 if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]); 00219 route[r] = 0; 00220 r--; 00221 } 00222 } 00223 r++; 00224 00225 motion.stop(); 00226 t1.reset(); 00227 t1.start(); 00228 while (t1 < 0.5f) {} 00229 t1.stop(); 00230 if (debuging == 1) printf("Loop stop\n"); 00231 00232 }else{ 00233 00234 route[r] = LEFT; 00235 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); 00236 r++; 00237 route[r] = MOVE; 00238 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); 00239 r++; 00240 00241 00242 motion.rotateL(); 00243 motion.scanMove(); 00244 } 00245 00246 //No wall center 00247 }else if (cWall == 0) { 00248 00249 if (route[r] == LEFT) { 00250 00251 route[r] = RIGHT; 00252 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); 00253 r++; 00254 route[r] = MOVE; 00255 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); 00256 r++; 00257 00258 motion.scanMove(); 00259 00260 }else if (route[r] == MOVE) { 00261 00262 int lastJunc = route[junction[j]]; 00263 00264 junction[j] = 0; 00265 if (debuging == 1) printf("Kreuzung %d Schritt %d geloscht\n", j, r); 00266 j -= 1; 00267 bool looping = false; 00268 00269 while (junction[j] <= r ) { 00270 00271 //invert rotation 00272 00273 if (junction[j] == r) { 00274 00275 switch (route[r]) { 00276 case MOVE: 00277 if (looping == false) { 00278 motion.runTask(route, r, true, junction[j]); 00279 if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]); 00280 } 00281 r--; 00282 break; 00283 case LEFT: 00284 r--; 00285 break; 00286 case RIGHT: 00287 r--; 00288 break; 00289 } 00290 motion.stop(); 00291 t1.reset(); 00292 t1.start(); 00293 while (t1 < 0.5f) {} 00294 t1.stop(); 00295 00296 }else{ 00297 if (route[r] == LEFT) { 00298 00299 route[r] = RIGHT; 00300 00301 }else if (route[r] == RIGHT) { 00302 00303 route[r] = LEFT; 00304 00305 }else if (junction[j] == r-1 && route[r] == MOVE) { 00306 00307 looping = true; 00308 00309 }else if (junction[j] == r-2 && route[r] == MOVE && route[junction[j]] != MOVE && lastJunc == MOVE) { 00310 00311 route[r] = 0; 00312 r--; 00313 } 00314 00315 motion.runTask(route, r, true, junction[j]); 00316 if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]); 00317 route[r] = 0; 00318 r--; 00319 } 00320 } 00321 r++; 00322 motion.stop(); 00323 t1.reset(); 00324 t1.start(); 00325 while (t1 < 0.5f) {} 00326 t1.stop(); 00327 if (debuging == 1) printf("Loop stop\n"); 00328 00329 }else{ 00330 00331 route[r] = MOVE; 00332 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); 00333 r++; 00334 00335 motion.scanMove(); 00336 } 00337 00338 //No wall right 00339 }else if (rWall == 0) { 00340 00341 route[r] = RIGHT; 00342 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); 00343 r++; 00344 route[r] = MOVE; 00345 if (debuging == 1) printf("Schritt: %d, Befehl: %d\n", r, route[r]); 00346 r++; 00347 00348 motion.rotateR(); 00349 motion.scanMove(); 00350 00351 //Dead end routine 00352 }else if ((lWall + cWall + rWall) == 3) { 00353 00354 motion.rotate180(); 00355 if (debuging == 1) printf("Sackgasse Schritt: %d\n", r); 00356 r--; 00357 t1.reset(); 00358 t1.start(); 00359 while (t1 < 0.5f) {} 00360 t1.stop(); 00361 //Return to last junction 00362 while (junction[j] <= r ) { 00363 00364 if (junction[j] == r) { 00365 00366 switch (route[r]) { 00367 case MOVE: 00368 motion.runTask(route, r, true, junction[j]); 00369 if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]); 00370 r--; 00371 break; 00372 case LEFT: 00373 r--; 00374 break; 00375 case RIGHT: 00376 r--; 00377 break; 00378 } 00379 motion.stop(); 00380 t1.reset(); 00381 t1.start(); 00382 while (t1 < 0.5f) {} 00383 t1.stop(); 00384 00385 }else{ 00386 //invert rotation 00387 if (route[r] == LEFT) { 00388 00389 route[r] = RIGHT; 00390 00391 }else if (route[r] == RIGHT) { 00392 00393 route[r] = LEFT; 00394 } 00395 //Run tasks in declining order 00396 motion.runTask(route, r, true, junction[j]); 00397 if (debuging == 1) printf("-Schritt: %d, Befehl: %d\n", r, route[r]); 00398 route[r] = 0; 00399 r--; 00400 } 00401 } 00402 r++; 00403 } 00404 00405 //finish line check 00406 if (motion.finish() == 1) { 00407 00408 Ziel = 1; 00409 r = 1; 00410 //Convert rotations to smooth turns 00411 while (route[r] != 0) { 00412 //Convert pattern MOVE - ROTATE - MOVE 00413 if (route[r-1] == MOVE && (route[r] == LEFT || route[r] == RIGHT) && route[r+1] == MOVE) { 00414 00415 route[r-1] = HALF_MOVE; 00416 route[r+1] = HALF_MOVE; 00417 00418 if (route[r] == LEFT) route[r] = TURN_LEFT; 00419 else route[r] = TURN_RIGHT; 00420 //Convert consequent ROTATION to a smooth TURN 00421 }else if (route[r-1] == HALF_MOVE && (route[r] == LEFT || route[r] == RIGHT) && route[r+1] == MOVE) { 00422 00423 route[r-1] = EMPTY; 00424 route[r+1] = HALF_MOVE; 00425 00426 if (route[r] == LEFT) route[r] = TURN_LEFT; 00427 else route[r] = TURN_RIGHT; 00428 00429 } 00430 00431 r++; 00432 } 00433 r = 0; 00434 start = 0; 00435 controller.counterReset(); 00436 myled = 1; 00437 }else{ 00438 Ziel = 0; 00439 } 00440 00441 00442 } 00443 00444 /*-------------------------------------------------------------------------* 00445 * 00446 * Speed run 00447 * 00448 *-------------------------------------------------------------------------*/ 00449 00450 while ( start == 1 && Ziel == 1 ) { 00451 00452 motion.runTask(route,r,false,junction[j]); 00453 r++; 00454 00455 if (route[r] == 0) { 00456 //Weg fertig 00457 motion.stop(); 00458 start = 0; 00459 myled = 0; 00460 } 00461 } 00462 00463 00464 00465 00466 } 00467 }
Generated on Wed Jul 13 2022 06:13:06 by
1.7.2