Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

main.cpp@20:20573f55a5fd, 2018-05-11 (annotated)

Committer:
luethale
Date:
Fri May 11 13:12:28 2018 +0000
Revision:
20:20573f55a5fd
Parent:
19:6cd6cc5c8b4c
Child:
21:41997651337a
Clean version without trash

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