Shortest Path of Line Maze - Curved and Zig-zag Track

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}