Components and supplies
Arduino UNO
Apps and platforms
Arduino IDE
Project description
Code
Line Maze Solving - Curved and Zig-zag Track
arduino
Just copy and paste this to your Arduino IDE. I am using right hand rule. Sorry that's all for now (12 Aug 2017 5:38AM) I don't have more time to update more of this now. Someone asked about the "O" in "void choosepath()", then I realized that I have to update the "void shortpath()". (10 May 2019 10:38 MYT)
1// R. Joko Musridho 2// Informatics Engineering – Faculty of Science and Technology 3// UIN SUSKA RIAU 4// Pekanbaru, Riau, Indonesia 5 6// this was my Bachelor’s Degree final project (viva voce on July 22nd, 2014) 7 8 9int sensor0 = A4; //leftmost sensor, pin A4 10int sensor1 = A3; 11int sensor2 = A2; 12int sensor3 = A1; 13int sensor4 = A0;// rightmost sensor, pin A0 14 15int mata0; // mata means “eye” 16int mata1; 17int mata2; 18int mata3; 19int mata4; 20 21int motorR = 4; //forward or reverse, pin 4 22int onR = 5; //magnitude of the power for the motor 23int onL = 6; //magnitude of the power for the motor 24int motorL = 7; //forward or reverse, pin 7 25 26int data; 27unsigned char s[5]; //array for sensors 28 29int pathlength; //variable to record the total of path length 30int readpath; //variable to call the path record 31char path[99]; //array for path record 32 33int threshold = 990; //threshold between black/white 34 35//led to show what the robot is recording (L/R/S/U) 36int BLUEled = 13; 37int GREENled = 12; 38int YELLOWled = 11; 39 40void strongright() 41{ 42 analogWrite(onL, 255); 43 digitalWrite(motorL, HIGH); 44 analogWrite(onR, 105); 45 digitalWrite(motorR, LOW);//to keep the robot on the line 46} 47 48void midright2() 49{ 50 analogWrite(onL, 255); 51 digitalWrite(motorL, HIGH); 52 analogWrite(onR, 105); 53 digitalWrite(motorR, HIGH); 54} 55 56void midright() 57{ 58 analogWrite(onL, 255); 59 digitalWrite(motorL, HIGH); 60 analogWrite(onR, 125); 61 digitalWrite(motorR, HIGH); 62} 63 64void softright() 65{ 66 analogWrite(onL, 255); 67 digitalWrite(motorL, HIGH); 68 analogWrite(onR, 225); 69 digitalWrite(motorR, HIGH); 70} 71 72void moveforward() 73{ 74 analogWrite(onL, 255); 75 digitalWrite(motorL, HIGH); 76 analogWrite(onR, 255); 77 digitalWrite(motorR, HIGH); 78} 79 80void softleft() 81{ 82 analogWrite(onL, 225); 83 digitalWrite(motorL, HIGH); 84 analogWrite(onR, 255); 85 digitalWrite(motorR, HIGH); 86} 87 88void midleft() 89{ 90 analogWrite(onL, 125); 91 digitalWrite(motorL, HIGH); 92 analogWrite(onR, 255); 93 digitalWrite(motorR, HIGH); 94} 95 96void midleft2() 97{ 98 analogWrite(onL, 105); 99 digitalWrite(motorL, HIGH); 100 analogWrite(onR, 255); 101 digitalWrite(motorR, HIGH); 102} 103 104void strongleft() 105{ 106 analogWrite(onL, 105); 107 digitalWrite(motorL, LOW);//to keep the robot on the line 108 analogWrite(onR, 255); 109 digitalWrite(motorR, HIGH); 110} 111 112void turnright() 113{ 114 analogWrite(onL, 255); 115 digitalWrite(motorL, HIGH); 116 analogWrite(onR, 255); 117 digitalWrite(motorR, LOW); 118} 119 120void turnleft() 121{ 122 analogWrite(onL, 255); 123 digitalWrite(motorL, LOW); 124 analogWrite(onR, 255); 125 digitalWrite(motorR, HIGH); 126} 127 128void righttillstraight() 129{ 130 turnright(); 131 delay(500); //to be free from the line if there is a straight intersection (exit 2) 132 readsensor(); 133 while (s[2]==0) 134 { 135 turnright();readsensor(); 136 } 137} 138 139void lefttillstraight() 140{ 141 turnleft(); 142 delay(500); //to be free from the line if there is a straight intersection (exit 2) 143 readsensor(); 144 while (s[2]==0) 145 { 146 turnleft();readsensor(); 147 } 148} 149 150void turnaround() 151{ 152 lefttillstraight(); 153} 154 155void stop() 156{ 157 analogWrite(onL, 0); 158 digitalWrite(motorL, LOW); 159 analogWrite(onR, 0); 160 digitalWrite(motorR, LOW); 161} 162 163void lilmoveforward() 164{ 165 moveforward(); 166 delay(150); // adjust based on your batteries power, new one would be stronger than the drained-out one, of course 167 readsensor(); 168} 169 170void ONforBLUEled() 171{ 172 digitalWrite (BLUEled, HIGH); delay(25); 173 digitalWrite (BLUEled, LOW); 174} 175 176void ONforGREENled() 177{ 178 digitalWrite (GREENled, HIGH); delay(25); 179 digitalWrite (GREENled, LOW); 180} 181 182void ONforYELLOWled() 183{ 184 digitalWrite (YELLOWled, HIGH); delay(25); 185 digitalWrite (YELLOWled, LOW); 186} 187 188void readsensor() 189{ 190 //if declared outside, the sensor readings become “11111” 191 mata0 = analogRead(sensor0); 192 mata1 = analogRead(sensor1); 193 mata2 = analogRead(sensor2); 194 mata3 = analogRead(sensor3); 195 mata4 = analogRead(sensor4); 196 197//converting to digital 198if (mata0 < threshold) 199 {s[0] = 1;} 200else 201 {s[0] = 0;} 202 203if (mata1 < threshold) 204 {s[1] = 1;} 205else 206 {s[1] = 0;} 207 208if (mata2 < threshold) 209 {s[2] = 1;} 210else 211 {s[2] = 0;} 212 213if (mata3 < threshold) 214 {s[3] = 1;} 215else 216 {s[3] = 0;} 217 218if (mata4 < threshold) 219 {s[4] = 1;} 220else 221 {s[4] = 0;} 222 223//change the sensor readings into a series of binary number 224data=(s[0]*16)+(s[1]*8)+(s[2]*4)+(s[3]*2)+(s[4]*1); 225 226//to display the sensor readings on serial monitor 227Serial.print(s[0]); 228Serial.print(" "); 229Serial.print(s[1]); 230Serial.print(" "); 231Serial.print(s[2]); 232Serial.print(" "); 233Serial.print(s[3]); 234Serial.print(" "); 235Serial.print(s[4]); 236Serial.println(" "); 237} 238 239//Intersection condition -------------- 0b00abcde 240void condition() 241{ 242 if (data==0b0000100) 243 {moveforward();} 244 else if (data==0b0000001) 245 { 246 strongright(); 247 readsensor(); 248 while (s[3]==0) 249 { 250 strongright(); 251 readsensor(); 252 } 253 } 254 else if (data==0b0000011) 255 {midright2();} 256 else if (data==0b0000010) 257 {midright();} 258 else if (data==0b0000110) 259 {softright();} 260 else if (data==0b0001100) 261 {softleft();} 262 else if (data==0b0001000) 263 {midleft();} 264 else if (data==0b0011000) 265 {midleft2();} 266 else if (data==0b0010000) 267 { 268 strongleft(); 269 readsensor(); 270 while (s[1]==0) 271 { 272 strongleft(); 273 readsensor(); 274 } 275 } 276 else //there is a right angle turn or intersection 277 { 278 if (data==0b0000000) //dead end 279 { 280 turnaround(); 281 path[pathlength]='U';pathlength++;//save U 282 ONforYELLOWled(); 283 } 284 else if (data==0b0011111) //T, +, end of maze 285 { 286 lilmoveforward(); 287 if (data==0b0000000)// T intersection 288 { 289 lilmoveforward(); //turning stabilizer 290 righttillstraight(); 291 path[pathlength]='R';pathlength++;//save R 292 ONforGREENled(); 293 } 294 else if (data==0b0011111)//end of maze 295 { 296 stop(); //stopping the robot 297 path[pathlength]='F';pathlength++;//save F 298 299 //sign for the end of maze 300 ONforBLUEled();delay(300); 301 ONforBLUEled();delay(300); 302 ONforYELLOWled();delay(300); 303 ONforYELLOWled();delay(300); 304 ONforGREENled();delay(300); 305 ONforGREENled();delay(300); 306 ONforBLUEled();ONforYELLOWled();ONforGREENled(); 307 308 shortpath(); //calculate the shortest path 309 ONforBLUEled();ONforYELLOWled();ONforGREENled(); 310 311 //sign to prepare the robot (put it back) on the starting position 312 ONforBLUEled();delay(300); 313 ONforYELLOWled();delay(1000); 314 ONforGREENled();delay(300); 315 ONforGREENled();delay(300); 316 ONforGREENled();delay(1000); 317 318 shortestpath(); //move through the shortest path 319 } 320 else //ada garis ke arah lurus 00100 dll (+ intersection) 321 { 322 lilmoveforward(); //turning stabilizer 323 righttillstraight(); 324 path[pathlength]='R';pathlength++;//save R 325 ONforGREENled(); 326 } 327 } 328 //straight or right junction 329 else if ((data==0b0000111)||(data==0b0001111)) 330 { 331 lilmoveforward();lilmoveforward(); 332 if (data==0b0000000)//right only 333 { 334 lilmoveforward(); 335 righttillstraight(); 336 } 337 else if (s[0]==0) 338 { 339 lilmoveforward(); 340 righttillstraight(); 341 path[pathlength]='R';pathlength++;//save R 342 ONforGREENled(); 343 } 344 } 345//left or straight junction 346 else if ((data==0b0011100)||(data==0b0011110)) 347 { 348 lilmoveforward();lilmoveforward(); 349 if (data==0b0000000) //left only 350 { 351 lilmoveforward(); 352 lefttillstraight(); 353 } 354 else if (s[4]==0) //there is a straight path 355 { 356 moveforward(); 357 path[pathlength]='S';pathlength++;//save S 358 ONforBLUEled(); 359 } 360 } 361 } 362} 363 364void shortpath() //calculate the shortest path 365{ 366 //because (..F) is the last and there is no U recorderd before F 367 int x = (pathlength-2); 368 369 while (x > 0) 370 { 371 if (path[x]=='U') 372 { 373 if (path[x-1]=='L' && path[x+1]=='L') 374 {path[x-1]='S';path[x]='O';path[x+1]='O';} 375 else if (path[x-1]=='L' && path[x+1]=='S') 376 {path[x-1]='R';path[x]='O';path[x+1]='O';} 377 else if (path[x-1]=='R' && path[x+1]=='R') 378 {path[x-1]='S';path[x]='O';path[x+1]='O';} 379 else if (path[x-1]=='R' && path[x+1]=='S') 380 {path[x-1]='L';path[x]='O';path[x+1]='O';} 381 else if (path[x-1]=='S' && path[x+1]=='L') 382 {path[x-1]='R';path[x]='O';path[x+1]='O';} 383 else if (path[x-1]=='S' && path[x+1]=='R') 384 {path[x-1]='L';path[x]='O';path[x+1]='O';} 385 else if (path[x-1]=='L' && path[x+1]=='R') 386 { 387 path[x-1]='U';path[x]='O';path[x+1]='O'; 388 389 } 390 else if (path[x-1]=='R' && path[x+1]=='L') 391 { 392 path[x-1]='U';path[x]='O';path[x+1]='O'; 393 if 394 } 395 else if (path[x-1]=='S' && path[x+1]=='S') 396 { 397 path[x-1]='U';path[x]='O';path[x+1]='O'; 398 399 } 400 //--------------------------------------- 401 x--; 402 } 403 else 404 {x--;} 405 } 406} 407 408void shortestpath() 409{ 410 readsensor(); 411 if (data==0b0000100) 412 {moveforward();} 413 else if (data==0b0000001) 414 { 415 strongright(); 416 readsensor(); 417 while (s[3]==0) 418 { 419 strongright(); 420 readsensor(); 421 } 422 } 423 else if (data==0b0000011) 424 {midright2();} 425 else if (data==0b0000010) 426 {midright();} 427 else if (data==0b0000110) 428 {softright();} 429 else if (data==0b0001100) 430 {softleft();} 431 else if (data==0b0001000) 432 {midleft();} 433 else if (data==0b0011000) 434 {midleft2();} 435 else if (data==0b0010000) 436 { 437 strongleft(); 438 readsensor(); 439 while (s[1]==0) 440 { 441 strongleft(); 442 readsensor(); 443 } 444 } 445 else 446 //right or straight 447 if ((data==0b0000111)||(data==0b0001111)) 448 { 449 lilmoveforward(); 450 if (data==0b0000000)//right only 451 { 452 lilmoveforward(); 453 righttillstraight(); 454 } 455 else //there is a straight path 456 { 457 choosepath(); 458 } 459 } 460 //left or straight 461 else if ((data==0b0011100)||(data==0b0011110)) 462 { 463 lilmoveforward(); 464 if (data==0b0000000) //left only 465 { 466 lilmoveforward(); 467 lefttillstraight(); 468 } 469 else //there is a straight path 470 { 471 choosepath(); 472 } 473 } 474 //T, +, or END OF MAZE 475 else if (data==0b0011111) 476 { 477 choosepath(); 478 } 479 shortestpath(); 480} 481 482void choosepath()//to get rid of the effect of “path[]==0” in the record 483{ 484 if (path[readpath]=='F') 485 { 486 stop();finish(); 487 } 488 else if (path[readpath]=='R') 489 { 490 ONforGREENled(); 491 righttillstraight(); 492 } 493 else if (path[readpath]=='S') 494 { 495 ONforGREENled();ONforBLUEled(); 496 moveforward();delay(200); 497 } 498 else if (path[readpath]=='L') 499 { 500 ONforGREENled();ONforYELLOWled(); 501 lefttillstraight(); 502 } 503 else if (path[readpath]=='O') 504 { 505 readpath++;choosepath(); 506 } 507 readpath++; 508 shortestpath(); 509} 510 511void finish() 512{ 513 ONforBLUEled();delay(100); 514 ONforYELLOWled();delay(100); 515 ONforGREENled();delay(100); 516 finish(); 517} 518 519void setup() 520{ 521 Serial.begin(9600); 522 pinMode(motorR, OUTPUT); 523 pinMode(motorL, OUTPUT); 524 pinMode(onR, OUTPUT); 525 pinMode(onL, OUTPUT); 526 pinMode(BLUEled, OUTPUT); 527 pinMode(YELLOWled, OUTPUT); 528 pinMode(GREENled, OUTPUT); 529 analogReference(INTERNAL); 530} 531 532void loop() 533{ 534 readsensor(); 535 condition(); 536}
Comments
Only logged in users can leave comments