Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

main.cpp@29:0419f4873807, 2018-05-18 (annotated)

Committer:
Helvis
Date:
Fri May 18 09:30:00 2018 +0000
Revision:
29:0419f4873807
Parent:
28:b7ce1e3bf08b 
v2.2d;

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