Helvijs Kiselis / Mbed 2 deprecated Micromouse

Algorithmus

Dependencies:   mbed

main.cpp@10:84534846e1f1, 2018-04-30 (annotated)

Committer:
luethale
Date:
Mon Apr 30 13:28:28 2018 +0000
Revision:
10:84534846e1f1
Parent:
9:a01f90f88920 
Clean up; volatile for; route[]; junction[]; j

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