Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

main.cpp@6:4868f789c223, 2018-04-24 (annotated)

Committer:
Helvis
Date:
Tue Apr 24 15:35:24 2018 +0000
Revision:
6:4868f789c223
Parent:
5:e2c0a4388d85
Child:
7:22392ed60534
v0.9.5 Funktionierende Suchfahrt

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