Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

main.cpp@1:2b5f79285a3e, 2018-04-19 (annotated)

Committer:
Helvis
Date:
Thu Apr 19 06:19:43 2018 +0000
Revision:
1:2b5f79285a3e
Parent:
0:8491169be8fc
Child:
2:f898adf2d817
Micromouse V0.9: Tresholds + Regler noch anpassen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Helvis 1:2b5f79285a3e 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 1:2b5f79285a3e 11 > Mitte Werte L/R
Helvis 1:2b5f79285a3e 12 - Suchfahrt testen
Helvis 1:2b5f79285a3e 13 > Tresholds bestimmen
Helvis 1:2b5f79285a3e 14 - gespeicherten Weg fahren
Helvis 1:2b5f79285a3e 15 - Problem: Links beschleunigt schneller
Helvis 1:2b5f79285a3e 16 - Ziellinie
Helvis 1:2b5f79285a3e 17 - Beschleunigung
Helvis 1:2b5f79285a3e 18 - über längere Strecke schneller fahren
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 const int STOP = 5;
Helvis 1:2b5f79285a3e 72
Helvis 1:2b5f79285a3e 73 //Sensor tresholds [mm]
Helvis 1:2b5f79285a3e 74 const float thresholdL = 70;
Helvis 1:2b5f79285a3e 75 const float thresholdR = 70;
Helvis 1:2b5f79285a3e 76 const float thresholdC = 100;
Helvis 0:8491169be8fc 77
Helvis 0:8491169be8fc 78 //------------------------------------------------------------------------------
Helvis 0:8491169be8fc 79
Helvis 0:8491169be8fc 80 //User button toggle
Helvis 0:8491169be8fc 81 void press() {
Helvis 0:8491169be8fc 82 start = !start;
Helvis 0:8491169be8fc 83 }
Helvis 1:2b5f79285a3e 84
Helvis 1:2b5f79285a3e 85 //Return to last junction
Helvis 1:2b5f79285a3e 86 void reverseToJunction(int& junc, int& r, int route[]) {
Helvis 1:2b5f79285a3e 87
Helvis 1:2b5f79285a3e 88 while (junc < r ) {
Helvis 1:2b5f79285a3e 89
Helvis 1:2b5f79285a3e 90 //invert rotation
Helvis 1:2b5f79285a3e 91 if (route[r] == LEFT) {
Helvis 1:2b5f79285a3e 92
Helvis 1:2b5f79285a3e 93 route[r] = RIGHT;
Helvis 1:2b5f79285a3e 94
Helvis 1:2b5f79285a3e 95 }else if (route[r] == RIGHT) {
Helvis 1:2b5f79285a3e 96
Helvis 1:2b5f79285a3e 97 route[r] = LEFT;
Helvis 1:2b5f79285a3e 98 }
Helvis 1:2b5f79285a3e 99
Helvis 1:2b5f79285a3e 100 motion.runTask(route[r]);
Helvis 1:2b5f79285a3e 101 route[r] = 0;
Helvis 1:2b5f79285a3e 102 r--;
Helvis 1:2b5f79285a3e 103 }
Helvis 1:2b5f79285a3e 104 }
Helvis 0:8491169be8fc 105
Helvis 0:8491169be8fc 106
Helvis 0:8491169be8fc 107 //------------------------------------------------------------------------------
Helvis 0:8491169be8fc 108
Helvis 0:8491169be8fc 109 int main() {
Helvis 1:2b5f79285a3e 110 //Init
Helvis 0:8491169be8fc 111
Helvis 1:2b5f79285a3e 112 int route[50] = {0};
Helvis 1:2b5f79285a3e 113 int r = 0;
Helvis 1:2b5f79285a3e 114
Helvis 1:2b5f79285a3e 115 int junction[5] = {0};
Helvis 1:2b5f79285a3e 116 int j = 0;
Helvis 0:8491169be8fc 117
Helvis 1:2b5f79285a3e 118 short lWall;
Helvis 1:2b5f79285a3e 119 short cWall;
Helvis 1:2b5f79285a3e 120 short rWall;
Helvis 0:8491169be8fc 121
Helvis 1:2b5f79285a3e 122 short Ziel = 0;
Helvis 1:2b5f79285a3e 123
Helvis 1:2b5f79285a3e 124 //loop
Helvis 0:8491169be8fc 125 while(1) {
Helvis 0:8491169be8fc 126
Helvis 0:8491169be8fc 127 button.fall(&press); //User button einlesen
Helvis 0:8491169be8fc 128
Helvis 1:2b5f79285a3e 129 /**
Helvis 1:2b5f79285a3e 130 *
Helvis 1:2b5f79285a3e 131 * Search run
Helvis 1:2b5f79285a3e 132 *
Helvis 1:2b5f79285a3e 133 */
Helvis 1:2b5f79285a3e 134
Helvis 1:2b5f79285a3e 135 while(start == 1 && Ziel == 0) {
Helvis 0:8491169be8fc 136
Helvis 1:2b5f79285a3e 137 /**
Helvis 1:2b5f79285a3e 138 * Entscheidung und Bewegung
Helvis 1:2b5f79285a3e 139 */
Helvis 1:2b5f79285a3e 140
Helvis 1:2b5f79285a3e 141
Helvis 0:8491169be8fc 142 float distanceL = irSensorL.readL();
Helvis 0:8491169be8fc 143 float distanceC = irSensorC.readC();
Helvis 0:8491169be8fc 144 float distanceR = irSensorR.readR();
Helvis 0:8491169be8fc 145
Helvis 1:2b5f79285a3e 146 //Wall check
Helvis 1:2b5f79285a3e 147 if (distanceL < thresholdL) lWall = 1;
Helvis 1:2b5f79285a3e 148 else lWall = 0;
Helvis 1:2b5f79285a3e 149 if (distanceC < thresholdC) cWall = 1;
Helvis 1:2b5f79285a3e 150 else cWall = 0;
Helvis 1:2b5f79285a3e 151 if (distanceR < thresholdR) rWall = 1;
Helvis 1:2b5f79285a3e 152 else rWall = 0;
Helvis 0:8491169be8fc 153
Helvis 1:2b5f79285a3e 154 //Junction Check
Helvis 1:2b5f79285a3e 155 if ((lWall + cWall + rWall) < 2) {
Helvis 0:8491169be8fc 156
Helvis 1:2b5f79285a3e 157 if (junction[j] > 0) {j++;}
Helvis 1:2b5f79285a3e 158 junction[j] = 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 1:2b5f79285a3e 166 r++;
Helvis 1:2b5f79285a3e 167
Helvis 1:2b5f79285a3e 168 motion.rotateL();
Helvis 1:2b5f79285a3e 169 motion.scanMove();
Helvis 1:2b5f79285a3e 170 motion.stop();
Helvis 1:2b5f79285a3e 171
Helvis 1:2b5f79285a3e 172 }else if (route[r] == MOVE) {
Helvis 1:2b5f79285a3e 173
Helvis 1:2b5f79285a3e 174 route[r] = RIGHT;
Helvis 1:2b5f79285a3e 175 r++;
Helvis 1:2b5f79285a3e 176 route[r] = MOVE;
Helvis 1:2b5f79285a3e 177 r++;
Helvis 1:2b5f79285a3e 178
Helvis 1:2b5f79285a3e 179 motion.rotateL();
Helvis 1:2b5f79285a3e 180 motion.scanMove();
Helvis 1:2b5f79285a3e 181 motion.stop();
Helvis 1:2b5f79285a3e 182
Helvis 1:2b5f79285a3e 183 }else if (route[r] == RIGHT) {
Helvis 1:2b5f79285a3e 184 // Kreuzung führt zu Sackgassen -> löschen
Helvis 1:2b5f79285a3e 185 junction[j] = 0;
Helvis 1:2b5f79285a3e 186 if (j > 0) {j--;}
Helvis 1:2b5f79285a3e 187 reverseToJunction(junction[j], r, route);
Helvis 1:2b5f79285a3e 188
Helvis 1:2b5f79285a3e 189 }else{
Helvis 1:2b5f79285a3e 190
Helvis 1:2b5f79285a3e 191 route[r] = LEFT;
Helvis 1:2b5f79285a3e 192 r++;
Helvis 1:2b5f79285a3e 193 route[r] = MOVE;
Helvis 1:2b5f79285a3e 194 r++;
Helvis 1:2b5f79285a3e 195
Helvis 1:2b5f79285a3e 196 motion.rotateL();
Helvis 1:2b5f79285a3e 197 motion.scanMove();
Helvis 1:2b5f79285a3e 198 motion.stop();
Helvis 1:2b5f79285a3e 199 }
Helvis 1:2b5f79285a3e 200 //No wall center
Helvis 1:2b5f79285a3e 201 }else if (cWall == 0) {
Helvis 0:8491169be8fc 202
Helvis 1:2b5f79285a3e 203 if (route[r] == LEFT) {
Helvis 1:2b5f79285a3e 204
Helvis 1:2b5f79285a3e 205 route[r] = RIGHT;
Helvis 1:2b5f79285a3e 206 r++;
Helvis 1:2b5f79285a3e 207 route[r] = MOVE;
Helvis 1:2b5f79285a3e 208 r++;
Helvis 0:8491169be8fc 209
Helvis 1:2b5f79285a3e 210 motion.scanMove();
Helvis 1:2b5f79285a3e 211 motion.stop();
Helvis 1:2b5f79285a3e 212
Helvis 1:2b5f79285a3e 213 }else if (route[r] == MOVE) {
Helvis 1:2b5f79285a3e 214
Helvis 1:2b5f79285a3e 215 junction[j] = 0;
Helvis 1:2b5f79285a3e 216 if (j > 0) {j--;}
Helvis 1:2b5f79285a3e 217 reverseToJunction(junction[j], r, route);
Helvis 1:2b5f79285a3e 218
Helvis 1:2b5f79285a3e 219 }else{
Helvis 1:2b5f79285a3e 220
Helvis 1:2b5f79285a3e 221 route[r] = MOVE;
Helvis 1:2b5f79285a3e 222 r++;
Helvis 1:2b5f79285a3e 223
Helvis 1:2b5f79285a3e 224 motion.scanMove();
Helvis 1:2b5f79285a3e 225 motion.stop();
Helvis 1:2b5f79285a3e 226 }
Helvis 1:2b5f79285a3e 227 //No wall right
Helvis 1:2b5f79285a3e 228 }else if (rWall == 0) {
Helvis 1:2b5f79285a3e 229
Helvis 1:2b5f79285a3e 230 route[r] = RIGHT;
Helvis 1:2b5f79285a3e 231 r++;
Helvis 1:2b5f79285a3e 232 route[r] = MOVE;
Helvis 1:2b5f79285a3e 233 r++;
Helvis 1:2b5f79285a3e 234
Helvis 1:2b5f79285a3e 235 motion.rotateR();
Helvis 1:2b5f79285a3e 236 motion.scanMove();
Helvis 1:2b5f79285a3e 237 motion.stop();
Helvis 1:2b5f79285a3e 238
Helvis 1:2b5f79285a3e 239 //Dead end routine
Helvis 1:2b5f79285a3e 240 }else if ((lWall + cWall + rWall) == 3) {
Helvis 1:2b5f79285a3e 241
Helvis 1:2b5f79285a3e 242 motion.rotate180();
Helvis 1:2b5f79285a3e 243 r--;
Helvis 1:2b5f79285a3e 244 //Return to last junction
Helvis 1:2b5f79285a3e 245 while (junction[j] < r ) {
Helvis 1:2b5f79285a3e 246
Helvis 1:2b5f79285a3e 247 //invert rotation
Helvis 1:2b5f79285a3e 248 if (route[r] == LEFT) {
Helvis 1:2b5f79285a3e 249
Helvis 1:2b5f79285a3e 250 route[r] = RIGHT;
Helvis 1:2b5f79285a3e 251
Helvis 1:2b5f79285a3e 252 }else if (route[r] == RIGHT) {
Helvis 1:2b5f79285a3e 253
Helvis 1:2b5f79285a3e 254 route[r] = LEFT;
Helvis 1:2b5f79285a3e 255 }
Helvis 1:2b5f79285a3e 256 //Run tasks in declining order
Helvis 1:2b5f79285a3e 257 motion.runTask(route[r]);
Helvis 1:2b5f79285a3e 258 route[r] = 0;
Helvis 1:2b5f79285a3e 259 r--;
Helvis 1:2b5f79285a3e 260 }
Helvis 0:8491169be8fc 261 }
Helvis 1:2b5f79285a3e 262
Helvis 1:2b5f79285a3e 263 if (motion.finish() == 1) {
Helvis 1:2b5f79285a3e 264 Ziel = 1;
Helvis 1:2b5f79285a3e 265 r = 0;
Helvis 1:2b5f79285a3e 266 start = 0;
Helvis 1:2b5f79285a3e 267 }else{
Helvis 1:2b5f79285a3e 268 Ziel = 0;
Helvis 0:8491169be8fc 269 }
Helvis 0:8491169be8fc 270
Helvis 0:8491169be8fc 271
Helvis 1:2b5f79285a3e 272 }
Helvis 0:8491169be8fc 273
Helvis 1:2b5f79285a3e 274 /**
Helvis 1:2b5f79285a3e 275 *
Helvis 1:2b5f79285a3e 276 * Speed run
Helvis 1:2b5f79285a3e 277 *
Helvis 1:2b5f79285a3e 278 */
Helvis 1:2b5f79285a3e 279
Helvis 1:2b5f79285a3e 280 while ( start == 1 && Ziel == 1 ) {
Helvis 0:8491169be8fc 281
Helvis 1:2b5f79285a3e 282 if (route[r] == route[r+1] && route[r] == route[r+2]) {
Helvis 1:2b5f79285a3e 283 //Auf längere Strecke schneller fahren
Helvis 1:2b5f79285a3e 284 route[r+1] = SPEED;
Helvis 0:8491169be8fc 285 }
Helvis 1:2b5f79285a3e 286 motion.runTask(route[r]);
Helvis 1:2b5f79285a3e 287 r++;
Helvis 0:8491169be8fc 288
Helvis 1:2b5f79285a3e 289 if (route[r] == 0) {
Helvis 1:2b5f79285a3e 290 //Weg fertig
Helvis 1:2b5f79285a3e 291 start = 0;
Helvis 1:2b5f79285a3e 292 }
Helvis 0:8491169be8fc 293 }
Helvis 0:8491169be8fc 294
Helvis 1:2b5f79285a3e 295
Helvis 1:2b5f79285a3e 296
Helvis 1:2b5f79285a3e 297
Helvis 0:8491169be8fc 298 }
Helvis 0:8491169be8fc 299 }
Helvis 0:8491169be8fc 300
Helvis 0:8491169be8fc 301
Helvis 0:8491169be8fc 302
Helvis 0:8491169be8fc 303