Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

main.cpp@23:accd07ca2da7, 2018-05-15 (annotated)

Committer:
Helvis
Date:
Tue May 15 13:35:50 2018 +0000
Revision:
23:accd07ca2da7
Parent:
21:41997651337a
Child:
28:b7ce1e3bf08b
v2.0 Turn implementiert; - Regelung muss mit 1 Seitenwand verbessert werden

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