├── Main Code └── MazeSolver.ino └── README.md /Main Code/MazeSolver.ino: -------------------------------------------------------------------------------- 1 | #define leftFarSensor A1 2 | #define leftNearSensor A2 3 | #define CenterSensor A3 4 | #define rightNearSensor A4 5 | #define rightFarSensor A5 6 | 7 | #define base 0 8 | #define line 1 9 | 10 | int CenterReading; 11 | int leftNearReading; 12 | int leftFarReading; 13 | int rightNearReading; 14 | int rightFarReading; 15 | 16 | int leftNudge; 17 | int replaystage; 18 | int rightNudge; 19 | 20 | #define leapTime 200 21 | 22 | #define leftMotor1 3 23 | #define leftMotor2 5 24 | 25 | #define rightMotor1 9 26 | #define rightMotor2 6 27 | 28 | #define enableA 10 29 | #define enableB 11 30 | 31 | #define botspeed 230 32 | 33 | #define led 13 34 | 35 | char path[30] = {}; 36 | int pathLength; 37 | int readLength; 38 | 39 | void setup(){ 40 | 41 | pinMode(leftNearSensor, INPUT); 42 | pinMode(leftFarSensor, INPUT); 43 | pinMode(CenterSensor, INPUT); 44 | pinMode(rightNearSensor, INPUT); 45 | pinMode(rightFarSensor, INPUT); 46 | 47 | pinMode(leftMotor1, OUTPUT); 48 | pinMode(leftMotor2, OUTPUT); 49 | pinMode(rightMotor1, OUTPUT); 50 | pinMode(rightMotor2, OUTPUT); 51 | pinMode(enableA, OUTPUT); 52 | pinMode(enableB, OUTPUT); 53 | 54 | analogWrite(enableA, botspeed); 55 | analogWrite(enableB, botspeed); 56 | 57 | pinMode(led, OUTPUT); 58 | //Serial.begin(115200); 59 | digitalWrite(led, HIGH); 60 | delay(1000); 61 | } 62 | 63 | 64 | void loop(){ 65 | 66 | readSensors(); 67 | 68 | if(leftFarReading==base && rightFarReading==base && (leftNearReading==line || CenterReading==line || rightNearReading==line)){ 69 | straight(); 70 | } 71 | else{ 72 | leftHand(); 73 | } 74 | 75 | } 76 | 77 | void readSensors(){ 78 | 79 | CenterReading = digitalRead(CenterSensor); 80 | leftNearReading = digitalRead(leftNearSensor); 81 | leftFarReading = digitalRead(leftFarSensor); 82 | rightNearReading = digitalRead(rightNearSensor); 83 | rightFarReading = digitalRead(rightFarSensor); 84 | 85 | // serial printing below for debugging purposes 86 | 87 | // Serial.print("CenterReading: "); 88 | // Serial.println(CenterReading); 89 | // Serial.print("leftNearReading: "); 90 | // Serial.println(leftNearReading); 91 | // Serial.print("leftFarReading: "); 92 | // Serial.println(leftFarReading); 93 | 94 | // Serial.print("rightNearReading: "); 95 | // Serial.println(rightNearReading); 96 | // Serial.print("rightFarReading: "); 97 | // Serial.println(rightFarReading); 98 | // delay(200); 99 | 100 | 101 | } 102 | 103 | 104 | void leftHand(){ 105 | 106 | 107 | if( leftFarReading==line && rightFarReading==line){ 108 | digitalWrite(leftMotor1, HIGH); 109 | digitalWrite(leftMotor2, LOW); 110 | digitalWrite(rightMotor1, HIGH); 111 | digitalWrite(rightMotor2, LOW); 112 | delay(leapTime); 113 | readSensors(); 114 | 115 | if(leftFarReading==line || rightFarReading==line){ 116 | done(); 117 | } 118 | if(leftFarReading==base && rightFarReading==base){ 119 | turnLeft(); 120 | } 121 | 122 | } 123 | 124 | if(leftFarReading==line){ // if you can turn left then turn left 125 | digitalWrite(leftMotor1, HIGH); 126 | digitalWrite(leftMotor2, LOW); 127 | digitalWrite(rightMotor1, HIGH); 128 | digitalWrite(rightMotor2, LOW); 129 | delay(leapTime); 130 | readSensors(); 131 | 132 | if(leftFarReading==base && rightFarReading==base){ 133 | turnLeft(); 134 | } 135 | else{ 136 | done(); 137 | } 138 | } 139 | 140 | if(rightFarReading==line){ 141 | digitalWrite(leftMotor1, HIGH); 142 | digitalWrite(leftMotor2, LOW); 143 | digitalWrite(rightMotor1, HIGH); 144 | digitalWrite(rightMotor2, LOW); 145 | delay(30); 146 | readSensors(); 147 | 148 | if(leftFarReading==line){ 149 | delay(leapTime-30); 150 | readSensors(); 151 | 152 | if(rightFarReading==line && leftFarReading==line){ 153 | done(); 154 | } 155 | else{ 156 | turnLeft(); 157 | return; 158 | } 159 | } 160 | delay(leapTime-30); 161 | readSensors(); 162 | if(leftFarReading==base && CenterReading==base && rightFarReading==base){ 163 | turnRight(); 164 | return; 165 | } 166 | path[pathLength]='S'; 167 | // Serial.println("s"); 168 | pathLength++; 169 | //Serial.print("Path length: "); 170 | //Serial.println(pathLength); 171 | if(path[pathLength-2]=='B'){ 172 | //Serial.println("shortening path"); 173 | shortPath(); 174 | } 175 | straight(); 176 | } 177 | readSensors(); 178 | if(leftFarReading==base && CenterReading==base && rightFarReading==base && leftNearReading==base && rightNearReading==base){ 179 | turnAround(); 180 | } 181 | 182 | } 183 | void done(){ 184 | digitalWrite(leftMotor1, LOW); 185 | digitalWrite(leftMotor2, LOW); 186 | digitalWrite(rightMotor1, LOW); 187 | digitalWrite(rightMotor2, LOW); 188 | replaystage=1; 189 | path[pathLength]='D'; 190 | pathLength++; 191 | while(digitalRead(leftFarSensor)==line){ 192 | digitalWrite(led, LOW); 193 | delay(150); 194 | digitalWrite(led, HIGH); 195 | delay(150); 196 | } 197 | delay(10000); 198 | replay(); 199 | } 200 | 201 | void turnLeft(){ 202 | 203 | 204 | 205 | while(digitalRead(CenterSensor)==line){ 206 | digitalWrite(leftMotor1, LOW); 207 | digitalWrite(leftMotor2, HIGH); 208 | digitalWrite(rightMotor1, HIGH); 209 | digitalWrite(rightMotor2, LOW); 210 | delay(2); 211 | digitalWrite(leftMotor1, LOW); 212 | digitalWrite(leftMotor2, LOW); 213 | digitalWrite(rightMotor1, LOW); 214 | digitalWrite(rightMotor2, LOW); 215 | delay(1); 216 | } 217 | 218 | while(digitalRead(CenterSensor)==base){ 219 | digitalWrite(leftMotor1, LOW); 220 | digitalWrite(leftMotor2, HIGH); 221 | digitalWrite(rightMotor1, HIGH); 222 | digitalWrite(rightMotor2, LOW); 223 | delay(2); 224 | digitalWrite(leftMotor1, LOW); 225 | digitalWrite(leftMotor2, LOW); 226 | digitalWrite(rightMotor1, LOW); 227 | digitalWrite(rightMotor2, LOW); 228 | delay(1); 229 | } 230 | 231 | if(replaystage==0){ 232 | path[pathLength]='L'; 233 | //Serial.println("l"); 234 | pathLength++; 235 | //Serial.print("Path length: "); 236 | //Serial.println(pathLength); 237 | if(path[pathLength-2]=='B'){ 238 | //Serial.println("shortening path"); 239 | shortPath(); 240 | } 241 | } 242 | } 243 | 244 | void turnRight(){ 245 | 246 | 247 | while(digitalRead(CenterSensor)==line){ 248 | digitalWrite(leftMotor1, HIGH); 249 | digitalWrite(leftMotor2, LOW); 250 | digitalWrite(rightMotor1, LOW); 251 | digitalWrite(rightMotor2, HIGH); 252 | delay(2); 253 | digitalWrite(leftMotor1, LOW); 254 | digitalWrite(leftMotor2, LOW); 255 | digitalWrite(rightMotor1, LOW); 256 | digitalWrite(rightMotor2, LOW); 257 | delay(1); 258 | } 259 | while(digitalRead(CenterSensor)==base){ 260 | digitalWrite(leftMotor1, HIGH); 261 | digitalWrite(leftMotor2, LOW); 262 | digitalWrite(rightMotor1, LOW); 263 | digitalWrite(rightMotor2, HIGH); 264 | delay(2); 265 | digitalWrite(leftMotor1, LOW); 266 | digitalWrite(leftMotor2, LOW); 267 | digitalWrite(rightMotor1, LOW); 268 | digitalWrite(rightMotor2, LOW); 269 | delay(1); 270 | } 271 | 272 | if(replaystage==0){ 273 | path[pathLength]='R'; 274 | //Serial.println("r"); 275 | pathLength++; 276 | //Serial.print("Path length: "); 277 | //Serial.println(pathLength); 278 | if(path[pathLength-2]=='B'){ 279 | //Serial.println("shortening path"); 280 | shortPath(); 281 | } 282 | } 283 | 284 | } 285 | 286 | void straight(){ 287 | if( digitalRead(rightNearSensor)==line){ 288 | digitalWrite(leftMotor1, HIGH); 289 | digitalWrite(leftMotor2, LOW); 290 | digitalWrite(rightMotor1, HIGH); 291 | digitalWrite(rightMotor2, LOW); 292 | delay(1); 293 | digitalWrite(leftMotor1, HIGH); 294 | digitalWrite(leftMotor2, LOW); 295 | digitalWrite(rightMotor1, LOW); 296 | digitalWrite(rightMotor2, LOW); 297 | delay(5); 298 | return; 299 | } 300 | if(digitalRead(leftNearSensor)==line){ 301 | digitalWrite(leftMotor1, HIGH); 302 | digitalWrite(leftMotor2, LOW); 303 | digitalWrite(rightMotor1, HIGH); 304 | digitalWrite(rightMotor2, LOW); 305 | delay(1); 306 | digitalWrite(leftMotor1, LOW); 307 | digitalWrite(leftMotor2, LOW); 308 | digitalWrite(rightMotor1, HIGH); 309 | digitalWrite(rightMotor2, LOW); 310 | delay(5); 311 | return; 312 | } 313 | 314 | digitalWrite(leftMotor1, HIGH); 315 | digitalWrite(leftMotor2, LOW); 316 | digitalWrite(rightMotor1, HIGH); 317 | digitalWrite(rightMotor2, LOW); 318 | delay(4); 319 | digitalWrite(leftMotor1, LOW); 320 | digitalWrite(leftMotor2, LOW); 321 | digitalWrite(rightMotor1, LOW); 322 | digitalWrite(rightMotor2, LOW); 323 | delay(1); 324 | 325 | } 326 | 327 | void turnAround(){ 328 | digitalWrite(leftMotor1, HIGH); 329 | digitalWrite(leftMotor2, LOW); 330 | digitalWrite(rightMotor1, HIGH); 331 | digitalWrite(rightMotor2, LOW); 332 | delay(150); 333 | while(digitalRead(CenterSensor)==base){ 334 | digitalWrite(leftMotor1, LOW); 335 | digitalWrite(leftMotor2, HIGH); 336 | digitalWrite(rightMotor1, HIGH); 337 | digitalWrite(rightMotor2, LOW); 338 | delay(2); 339 | digitalWrite(leftMotor1, LOW); 340 | digitalWrite(leftMotor2, LOW); 341 | digitalWrite(rightMotor1, LOW); 342 | digitalWrite(rightMotor2, LOW); 343 | delay(1); 344 | } 345 | path[pathLength]='B'; 346 | pathLength++; 347 | straight(); 348 | //Serial.println("b"); 349 | //Serial.print("Path length: "); 350 | //Serial.println(pathLength); 351 | } 352 | 353 | void shortPath(){ 354 | int shortDone=0; 355 | if(path[pathLength-3]=='L' && path[pathLength-1]=='R'){ 356 | pathLength-=3; 357 | path[pathLength]='B'; 358 | //Serial.println("test1"); 359 | shortDone=1; 360 | } 361 | 362 | if(path[pathLength-3]=='L' && path[pathLength-1]=='S' && shortDone==0){ 363 | pathLength-=3; 364 | path[pathLength]='R'; 365 | //Serial.println("test2"); 366 | shortDone=1; 367 | } 368 | 369 | if(path[pathLength-3]=='R' && path[pathLength-1]=='L' && shortDone==0){ 370 | pathLength-=3; 371 | path[pathLength]='B'; 372 | //Serial.println("test3"); 373 | shortDone=1; 374 | } 375 | 376 | 377 | if(path[pathLength-3]=='S' && path[pathLength-1]=='L' && shortDone==0){ 378 | pathLength-=3; 379 | path[pathLength]='R'; 380 | //Serial.println("test4"); 381 | shortDone=1; 382 | } 383 | 384 | if(path[pathLength-3]=='S' && path[pathLength-1]=='S' && shortDone==0){ 385 | pathLength-=3; 386 | path[pathLength]='B'; 387 | //Serial.println("test5"); 388 | shortDone=1; 389 | } 390 | if(path[pathLength-3]=='L' && path[pathLength-1]=='L' && shortDone==0){ 391 | pathLength-=3; 392 | path[pathLength]='S'; 393 | //Serial.println("test6"); 394 | shortDone=1; 395 | } 396 | 397 | path[pathLength+1]='D'; 398 | path[pathLength+2]='D'; 399 | pathLength++; 400 | //Serial.print("Path length: "); 401 | //Serial.println(pathLength); 402 | //printPath(); 403 | } 404 | 405 | 406 | 407 | 408 | void printPath(){ 409 | Serial.println("+++++++++++++++++"); 410 | int x; 411 | while(x<=pathLength){ 412 | Serial.println(path[x]); 413 | x++; 414 | } 415 | Serial.println("+++++++++++++++++"); 416 | } 417 | 418 | 419 | void replay(){ 420 | readSensors(); 421 | if(leftFarReading==base && rightFarReading==base){ 422 | straight(); 423 | } 424 | else{ 425 | if(path[readLength]=='D'){ 426 | digitalWrite(leftMotor1, HIGH); 427 | digitalWrite(leftMotor2, LOW); 428 | digitalWrite(rightMotor1, HIGH); 429 | digitalWrite(rightMotor2, LOW); 430 | delay(100); 431 | digitalWrite(leftMotor1, LOW); 432 | digitalWrite(leftMotor2, LOW); 433 | digitalWrite(rightMotor1, LOW); 434 | digitalWrite(rightMotor2, LOW); 435 | endMotion(); 436 | } 437 | if(path[readLength]=='L'){ 438 | digitalWrite(leftMotor1, HIGH); 439 | digitalWrite(leftMotor2, LOW); 440 | digitalWrite(rightMotor1, HIGH); 441 | digitalWrite(rightMotor2, LOW); 442 | delay(leapTime); 443 | turnLeft(); 444 | } 445 | if(path[readLength]=='R'){ 446 | digitalWrite(leftMotor1, HIGH); 447 | digitalWrite(leftMotor2, LOW); 448 | digitalWrite(rightMotor1, HIGH); 449 | digitalWrite(rightMotor2, LOW); 450 | delay(leapTime); 451 | turnRight(); 452 | } 453 | if(path[readLength]=='S'){ 454 | digitalWrite(leftMotor1, HIGH); 455 | digitalWrite(leftMotor2, LOW); 456 | digitalWrite(rightMotor1, HIGH); 457 | digitalWrite(rightMotor2, LOW); 458 | delay(leapTime); 459 | straight(); 460 | } 461 | 462 | readLength++; 463 | } 464 | 465 | replay(); 466 | 467 | } 468 | 469 | void endMotion(){ 470 | digitalWrite(led, LOW); 471 | delay(500); 472 | digitalWrite(led, HIGH); 473 | delay(200); 474 | digitalWrite(led, LOW); 475 | delay(200); 476 | digitalWrite(led, HIGH); 477 | delay(500); 478 | endMotion(); 479 | } 480 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Arduino-Maze-Solving-Robot 2 | 3 | An Arduino micro controller based robot which first analyzes the maze in the dry run by following lines through IR sensors and then calculates the shortest path from the source to the destination. 4 | 5 | ![Maze](https://user-images.githubusercontent.com/73630123/115738487-b9171e80-a3aa-11eb-8f36-94386b456330.jpg) 6 | 7 | The new array of now 6 sensors is mounted out of which only 5 sensors are used for better precision. If one sensor is centered with relation to the black line, only that specific sensor will produce a HIGH. The possible 5 original sensor array output when following a line are: 8 | [1 1 1 1 1, 0 0 0 0 0, 0 0 0 0 1, 0 0 0 1 1, 0 0 0 1 0, 0 0 1 1 0, 0 0 1 0 0 , 0 1 1 0 0 , 0 1 0 0 0, 1 1 0 0 0, 1 0 0 0 0] 9 | 10 | The maze solver bot code is split in two parts: 11 | 12 | 1. (First Part): The robot finds its way out from a "non-known perfect maze". Does not matter where you put it inside the maze, it will find a "solution". 13 | 2. (Second Part): Once the robot found a possible maze solution, it should optimize its solution finding the "shortest path from start to finish". 14 | 15 | To find the solution of the maze, the bot uses "Right Hand Rule". If in the maze all its walls are connected together maze's outer boundary, then by keeping one hand in contact with one wall of the maze, the solver will not to get lost and will reach the exit. Steps in Right Hand Rule are: 16 | 17 | 1. At "Cross": Go to Right 18 | 2. At "T": Go to Right 19 | 3. At "Right Only": Go to Right 20 | 4. At "Left Only": Go to Left 21 | 5. At "Straight or Left": Go Straight 22 | 6. At "Straight or Right": Go Right 23 | 7. At "Dead End": Go back ("U turn") 24 | 8. At the "End of Maze": Stop 25 | 26 | ![Screenshot_20210422-183759](https://user-images.githubusercontent.com/73630123/115738403-a6044e80-a3aa-11eb-8a28-ddb5c98389ec.jpg) 27 | --------------------------------------------------------------------------------