Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

main.cpp@21:41997651337a, 2018-05-12 (annotated)

Committer:
Helvis
Date:
Sat May 12 16:30:18 2018 +0000
Revision:
21:41997651337a
Parent:
20:20573f55a5fd
Child:
23:accd07ca2da7
v1.1: +unerwartete Beschleunigung korrigiert; - Langstreckenfahrt R?ckweg

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