Mazemaster3000

A finite state machine-powered maze solver that stores a log of steps without any external storage device.

Jul 5, 2019

•

5759 views

•

3 respects

toys

cars

Components and supplies

Jumper wires (generic)

Capacitor 10 µF

4xAA battery holder

Ultrasonic Sensor - HC-SR04 (Generic)

Robotic car kit

Pushbutton Switch, Momentary

Resistor 10k ohm

Slide Switch

AA Batteries

SparkFun Full-Bridge Motor Driver Breakout - L298N

Arduino UNO

Tools and machines

Multitool, Screwdriver

Tape, Electrical

Hot glue gun (generic)

Project description

Code

MazeMaster.ino

c_cpp

1//Authors: Gonçalo Azinheira and João Castro, students at University of  the Algarve(Ualg);
2//Project: Mazemaster3000;
3//Subject: A finite state machine-powered  maze solver. The starting point of the maze must be in one of the left corners and  the ending must be in one of the right corners.
4
5//----------------------------------------------------------------------------
6
7const  int pingt = 7; //the same trigger is used by all sensors
8const int pinge2 = 6;  //front ultrassonic sensor
9const int pinge1 = 8; //right ultrassonic sensor
10const  int pinge3 = 9; //left ultrassonic sensor
11int rec[50];
12int a;
13int In1 =  A5; //right
14int In2 = A4; //right
15int In3 = A3; //left
16int In4 = A2; //left
17int  EnA = 10; //right
18int EnB = 11; //left
19int dir;
20int esqdist;
21int dirdist;
22int  fdist;
23boolean button=0;
24
25void B_ISR(){ //The button is activated by an  Interrupt System Routine
26    button=1;
27 }  
28
29void setup() {
30Serial.begin(9600);
31//button=0;
32pinMode(button,  INPUT);
33attachInterrupt(0,B_ISR,RISING);
34
35pinMode(pingt, OUTPUT);
36pinMode(pinge1,  INPUT);
37pinMode(pinge2, INPUT);
38pinMode(pinge3, INPUT);
39a=0;
40pinMode(In1,  OUTPUT);
41pinMode(In2, OUTPUT);
42pinMode(In3, OUTPUT);
43pinMode(In4, OUTPUT);
44pinMode(EnA,  OUTPUT);
45pinMode(EnB, OUTPUT);
46pinMode(LED_BUILTIN, OUTPUT);
47analogWrite(EnA,  200);
48  analogWrite(EnB, 200);
49}
50
51
52//******************************************
53//---------------------------------------------------
54int  ping (int pinge){
55    long duration, cm;
56    int x,xmed;
57    int dela=25;
58    int MAX = 1500;
59    int num = 0;
60    int sum=0;
61
62    for(int i=0;i<=5;i++){
63  digitalWrite(pingt, LOW);
64  delayMicroseconds(2);
65  digitalWrite(pingt,  HIGH);
66  delayMicroseconds(5);
67  digitalWrite(pingt, LOW);
68  duration =  pulseIn(pinge, HIGH);
69  x = microsecondsToCentimeters(duration);
70  if(x<MAX){
71    num ++;
72  }
73  else{x=0;}
74  sum+=x;
75  
76  delay(dela);
77  }
78
79  xmed=sum/num;
80  return xmed;
81  } 
82
83 
84
85  long microsecondsToCentimeters(long  microseconds) {
86  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
87  // The ping travels out and back, so to find the distance of the object we
88  // take half of the distance travelled.
89  return microseconds / 29 / 2;
90}
91
92//-----------------------------------------------
93//**********************************
94//Each  funcion uses different values in the analogWrite and delay. That's because the motors  aren't exactly the same and the battery will slightly lose power.
95
96
97void  walk(int wtime) //Makes the cars go straight
98{
99  analogWrite(EnA, 173); 
100  analogWrite(EnB, 171);
101  digitalWrite(In1, HIGH);
102  digitalWrite(In2, LOW);
103  digitalWrite(In3, HIGH);
104  digitalWrite(In4, LOW);
105  delay(wtime*1.8);
106  digitalWrite(In1, LOW);
107  digitalWrite(In2, LOW);
108  digitalWrite(In3, LOW);
109  digitalWrite(In4, LOW);
110  delay(5);
111}
112
113
114
115  void turnR(int wtime)  //Makes the car turn right and then go straight
116  {
117  analogWrite(EnA, 192);
118  analogWrite(EnB, 172);
119  digitalWrite(In1, LOW);
120  digitalWrite(In2, HIGH);
121  digitalWrite(In3, HIGH);
122  digitalWrite(In4, LOW);
123  delay(wtime-141);
124  analogWrite(EnA, 170);
125  analogWrite(EnB, 170);
126  digitalWrite(In1, HIGH);
127  digitalWrite(In2, LOW);
128  digitalWrite(In3, HIGH);
129  digitalWrite(In4, LOW);
130  delay(wtime*2+50);
131  digitalWrite(In1, LOW);
132  digitalWrite(In2, LOW);
133  digitalWrite(In3, LOW);
134  digitalWrite(In4, LOW);
135  delay(5);
136    }
137
138  void turnL(int wtime) //Makes the car turn left and then go straight
139  {
140  analogWrite(EnA, 170);
141  analogWrite(EnB, 170);
142  digitalWrite(In1, HIGH);
143  digitalWrite(In2, LOW);
144  digitalWrite(In3, LOW);
145  digitalWrite(In4, HIGH);
146  delay(wtime-130);
147  analogWrite(EnA, 170);
148  analogWrite(EnB, 170);
149  digitalWrite(In1, HIGH);
150  digitalWrite(In2, LOW);
151  digitalWrite(In3, HIGH);
152  digitalWrite(In4, LOW);
153  delay(wtime*2+30);
154  digitalWrite(In1, LOW);
155  digitalWrite(In2, LOW);
156  digitalWrite(In3, LOW);
157  digitalWrite(In4, LOW);
158  delay(5);
159    }
160
161    void back(int wtime) //Makes the car turn around  180. This happens when the sensor detect obstacles in all directions.
162  {
163  analogWrite(EnA, 170);
164  analogWrite(EnB, 170);
165  digitalWrite(In1, LOW);
166  digitalWrite(In2, HIGH);
167  digitalWrite(In3, HIGH);
168  digitalWrite(In4,  LOW);
169  delay(wtime+220);
170  digitalWrite(In1, LOW);
171  digitalWrite(In2,  LOW);
172  digitalWrite(In3, LOW);
173  digitalWrite(In4, LOW);
174  delay(5);
175  }
176
177//**********************************
178//-----------------------------------------------
179
180//This  is the implementation of the finit state machine shown in the picture. It starts  always in state -1 and stays there until the button is pressed. After that, it will  go to state 0
181//and measures the distances using the ultrassonic sensors using  the pings() function. According to what it measured, the finite state machine will  go to the corresponding state using the
182//dir value (the meaning of this variable  is explained below in the pings() function). The states 1 to 4 are responsible for  making the car turn right, go straight, turn left or turn around respectively.
183//At  each of those states, the first thing that is done is record a value in the array  rec []. This value is equal to the the present state of the machine. For example,  if the car is going
184//straight ahead, it is in state 2 and the value recored  in the array will be also 2. In other words, the array stores the actions done by  the car to be written in the function Recb().
185//The cars reaches to the end if  it does 3 lefts in a row and to do so, it needs to go to state 3, state 8 and 5  consecutively. So state 8 represents the 2nd left and state 5 represents the
186//3rd  left. After reaching to the end of the maze, it goes from state 5 to 6. By this  point, the finite state machine will be always moving from state 6 to 7 and vice-versa.
187//State  6 turns on the arduino's builtin LED and state 7 turns it off. Regardless of if  it is in 6 or 7, it waits for the button to be pressed to print the log.
188
189void  MazeMaster_FSM(){
190static int state=-1; //The initial state is always -1 until  the button is pressed.
191const int D = 20;  //This is the reference distance, in  cm, for the car to consider the obstacles. If the sensor detects something below  this distance, it will consider as an obstacle.
192const int wtime = 500;  //Time  used on each action(right, straight, left or turn around).
193
194 
195
196  switch(state){
197
198    case -1:
199    if(button==1)
200    {button=0;
201    state=0;}
202    break;
203    
204    case 0:
205   dir=pings();
206   state=dir;
207    break;
208
209    case  1:
210    Serial.print("state = ");
211    Serial.println(state);
212    rec[a]=1;
213    a++;
214    turnR(wtime);
215    
216    dir=pings();
217   state=dir;
218    break;
219    
220    case 2:
221    Serial.print("state = ");
222    Serial.println(state);
223    rec[a]=2;
224    a++;
225    Serial.print("a2 = ");
226    Serial.println(a);
227    walk(wtime);
228    
229    dir=pings();
230   state=dir;
231    break;
232    
233    case 3:
234    Serial.print("state = ");
235    Serial.println(state);
236    rec[a]=3;
237    a++;
238    turnL(wtime);
239    
240    dir=pings();
241    if(dir==3)state=8;
242    else state=dir;
243    break;
244
245    case 4:
246    Serial.print("state  = ");
247    Serial.println(state);
248    rec[a]=4;
249    a++;
250    Serial.print("a4  = ");
251    Serial.println(a);
252    back(wtime);
253
254
255    dir=pings();
256    state=dir;
257    break;
258
259    case 5:
260    Serial.print("state = ");
261    Serial.println(state);
262    rec[a]=3;
263    a++;
264    turnL(wtime);
265    Serial.println("I HAVE ARRIVED! ");
266    rec[a]=5;
267    state=6;
268    break;
269    
270     case 6:
271     digitalWrite(LED_BUILTIN,HIGH);
272    if(button==1)
273    {Recb();
274    button==0;}
275     delay(500);
276    state=7;
277     break;
278     
279     case 7:
280     digitalWrite(LED_BUILTIN,LOW);
281    
282    if(button==1)
283    {Recb();
284    button==0;}
285    delay(500);
286    state=6;
287     break;
288
289     case 8:
290    Serial.print("state = ");
291    Serial.println(state);
292    rec[a]=3;
293    a++;
294    turnL(wtime);
295
296    dir=pings();
297    if(dir==3)state=5;
298    else state=dir;
299    break;
300
301    
302  }
303
304}
305//------------------------------
306
307void  Recb(){
308  Serial.print("start -> ");
309  for(int i=0;i<=a;i++)
310  {Serial.println(rec[i]);}
311  button=0;
312    }
313    //---------------------------
314
315//This is the function  that measures the distances using the ultrassonic sensors.
316//As the ending of  the maze is always in the right corner, the function will give priority the right  . It will only start measuring the front distance
317//if the right sensor detects  an obstacle (values below 20cm). The same goes for the left sensor, it will only  start measuring if the right and front sensors detect an obstacle.
318//This type  of functioning prevents the car to waste time in measuring distances that aren't  be necessary.
319//The function pings() returns a value, called dir, between 1 and  4. 1 - No obstacle on the right, 2 - Obstacle on the right, 3 - Obstacles in front  and right and 4 - Obstacles in all directions.
320    
321int pings(){
322const  int D = 20; 
323  
324    dirdist = ping(pinge1);
325    Serial.print("distance  at right(->) = ");
326    Serial.println(dirdist);
327    if(dirdist>D){dir=1;}
328    else{
329    fdist = ping(pinge2);
330    Serial.print("distance at front(^)  = ");
331    Serial.println(fdist);
332    if(fdist>D){dir=2;}
333    else{ 
334    esqdist = ping(pinge3);
335    Serial.print("distance at left(<-) = ");
336    Serial.println(esqdist);
337    if(esqdist>D){dir=3;}
338    else {dir=4;}
339    }
340    }
341return dir;
342}
343
344
345
346
347
348
349    //---------------------------
350
351void  loop() {  
352MazeMaster_FSM();
353}
354
355  
356

//Authors: Gonçalo Azinheira and João Castro, students at University of  the Algarve(Ualg);
//Project: Mazemaster3000;
//Subject: A finite state machine-powered  maze solver. The starting point of the maze must be in one of the left corners and  the ending must be in one of the right corners.

//----------------------------------------------------------------------------

const  int pingt = 7; //the same trigger is used by all sensors
const int pinge2 = 6;  //front ultrassonic sensor
const int pinge1 = 8; //right ultrassonic sensor
const  int pinge3 = 9; //left ultrassonic sensor
int rec[50];
int a;
int In1 =  A5; //right
int In2 = A4; //right
int In3 = A3; //left
int In4 = A2; //left
int  EnA = 10; //right
int EnB = 11; //left
int dir;
int esqdist;
int dirdist;
int  fdist;
boolean button=0;

void B_ISR(){ //The button is activated by an  Interrupt System Routine
    button=1;
 }  

void setup() {
Serial.begin(9600);
//button=0;
pinMode(button,  INPUT);
attachInterrupt(0,B_ISR,RISING);

pinMode(pingt, OUTPUT);
pinMode(pinge1,  INPUT);
pinMode(pinge2, INPUT);
pinMode(pinge3, INPUT);
a=0;
pinMode(In1,  OUTPUT);
pinMode(In2, OUTPUT);
pinMode(In3, OUTPUT);
pinMode(In4, OUTPUT);
pinMode(EnA,  OUTPUT);
pinMode(EnB, OUTPUT);
pinMode(LED_BUILTIN, OUTPUT);
analogWrite(EnA,  200);
  analogWrite(EnB, 200);
}


//******************************************
//---------------------------------------------------
int  ping (int pinge){
    long duration, cm;
    int x,xmed;
    int dela=25;
    int MAX = 1500;
    int num = 0;
    int sum=0;

    for(int i=0;i<=5;i++){
  digitalWrite(pingt, LOW);
  delayMicroseconds(2);
  digitalWrite(pingt,  HIGH);
  delayMicroseconds(5);
  digitalWrite(pingt, LOW);
  duration =  pulseIn(pinge, HIGH);
  x = microsecondsToCentimeters(duration);
  if(x<MAX){
    num ++;
  }
  else{x=0;}
  sum+=x;
  
  delay(dela);
  }

  xmed=sum/num;
  return xmed;
  } 

 

  long microsecondsToCentimeters(long  microseconds) {
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the object we
  // take half of the distance travelled.
  return microseconds / 29 / 2;
}

//-----------------------------------------------
//**********************************
//Each  funcion uses different values in the analogWrite and delay. That's because the motors  aren't exactly the same and the battery will slightly lose power.


void  walk(int wtime) //Makes the cars go straight
{
  analogWrite(EnA, 173); 
  analogWrite(EnB, 171);
  digitalWrite(In1, HIGH);
  digitalWrite(In2, LOW);
  digitalWrite(In3, HIGH);
  digitalWrite(In4, LOW);
  delay(wtime*1.8);
  digitalWrite(In1, LOW);
  digitalWrite(In2, LOW);
  digitalWrite(In3, LOW);
  digitalWrite(In4, LOW);
  delay(5);
}



  void turnR(int wtime)  //Makes the car turn right and then go straight
  {
  analogWrite(EnA, 192);
  analogWrite(EnB, 172);
  digitalWrite(In1, LOW);
  digitalWrite(In2, HIGH);
  digitalWrite(In3, HIGH);
  digitalWrite(In4, LOW);
  delay(wtime-141);
  analogWrite(EnA, 170);
  analogWrite(EnB, 170);
  digitalWrite(In1, HIGH);
  digitalWrite(In2, LOW);
  digitalWrite(In3, HIGH);
  digitalWrite(In4, LOW);
  delay(wtime*2+50);
  digitalWrite(In1, LOW);
  digitalWrite(In2, LOW);
  digitalWrite(In3, LOW);
  digitalWrite(In4, LOW);
  delay(5);
    }

  void turnL(int wtime) //Makes the car turn left and then go straight
  {
  analogWrite(EnA, 170);
  analogWrite(EnB, 170);
  digitalWrite(In1, HIGH);
  digitalWrite(In2, LOW);
  digitalWrite(In3, LOW);
  digitalWrite(In4, HIGH);
  delay(wtime-130);
  analogWrite(EnA, 170);
  analogWrite(EnB, 170);
  digitalWrite(In1, HIGH);
  digitalWrite(In2, LOW);
  digitalWrite(In3, HIGH);
  digitalWrite(In4, LOW);
  delay(wtime*2+30);
  digitalWrite(In1, LOW);
  digitalWrite(In2, LOW);
  digitalWrite(In3, LOW);
  digitalWrite(In4, LOW);
  delay(5);
    }

    void back(int wtime) //Makes the car turn around  180. This happens when the sensor detect obstacles in all directions.
  {
  analogWrite(EnA, 170);
  analogWrite(EnB, 170);
  digitalWrite(In1, LOW);
  digitalWrite(In2, HIGH);
  digitalWrite(In3, HIGH);
  digitalWrite(In4,  LOW);
  delay(wtime+220);
  digitalWrite(In1, LOW);
  digitalWrite(In2,  LOW);
  digitalWrite(In3, LOW);
  digitalWrite(In4, LOW);
  delay(5);
  }

//**********************************
//-----------------------------------------------

//This  is the implementation of the finit state machine shown in the picture. It starts  always in state -1 and stays there until the button is pressed. After that, it will  go to state 0
//and measures the distances using the ultrassonic sensors using  the pings() function. According to what it measured, the finite state machine will  go to the corresponding state using the
//dir value (the meaning of this variable  is explained below in the pings() function). The states 1 to 4 are responsible for  making the car turn right, go straight, turn left or turn around respectively.
//At  each of those states, the first thing that is done is record a value in the array  rec []. This value is equal to the the present state of the machine. For example,  if the car is going
//straight ahead, it is in state 2 and the value recored  in the array will be also 2. In other words, the array stores the actions done by  the car to be written in the function Recb().
//The cars reaches to the end if  it does 3 lefts in a row and to do so, it needs to go to state 3, state 8 and 5  consecutively. So state 8 represents the 2nd left and state 5 represents the
//3rd  left. After reaching to the end of the maze, it goes from state 5 to 6. By this  point, the finite state machine will be always moving from state 6 to 7 and vice-versa.
//State  6 turns on the arduino's builtin LED and state 7 turns it off. Regardless of if  it is in 6 or 7, it waits for the button to be pressed to print the log.

void  MazeMaster_FSM(){
static int state=-1; //The initial state is always -1 until  the button is pressed.
const int D = 20;  //This is the reference distance, in  cm, for the car to consider the obstacles. If the sensor detects something below  this distance, it will consider as an obstacle.
const int wtime = 500;  //Time  used on each action(right, straight, left or turn around).

 

  switch(state){

    case -1:
    if(button==1)
    {button=0;
    state=0;}
    break;
    
    case 0:
   dir=pings();
   state=dir;
    break;

    case  1:
    Serial.print("state = ");
    Serial.println(state);
    rec[a]=1;
    a++;
    turnR(wtime);
    
    dir=pings();
   state=dir;
    break;
    
    case 2:
    Serial.print("state = ");
    Serial.println(state);
    rec[a]=2;
    a++;
    Serial.print("a2 = ");
    Serial.println(a);
    walk(wtime);
    
    dir=pings();
   state=dir;
    break;
    
    case 3:
    Serial.print("state = ");
    Serial.println(state);
    rec[a]=3;
    a++;
    turnL(wtime);
    
    dir=pings();
    if(dir==3)state=8;
    else state=dir;
    break;

    case 4:
    Serial.print("state  = ");
    Serial.println(state);
    rec[a]=4;
    a++;
    Serial.print("a4  = ");
    Serial.println(a);
    back(wtime);


    dir=pings();
    state=dir;
    break;

    case 5:
    Serial.print("state = ");
    Serial.println(state);
    rec[a]=3;
    a++;
    turnL(wtime);
    Serial.println("I HAVE ARRIVED! ");
    rec[a]=5;
    state=6;
    break;
    
     case 6:
     digitalWrite(LED_BUILTIN,HIGH);
    if(button==1)
    {Recb();
    button==0;}
     delay(500);
    state=7;
     break;
     
     case 7:
     digitalWrite(LED_BUILTIN,LOW);
    
    if(button==1)
    {Recb();
    button==0;}
    delay(500);
    state=6;
     break;

     case 8:
    Serial.print("state = ");
    Serial.println(state);
    rec[a]=3;
    a++;
    turnL(wtime);

    dir=pings();
    if(dir==3)state=5;
    else state=dir;
    break;

    
  }

}
//------------------------------

void  Recb(){
  Serial.print("start -> ");
  for(int i=0;i<=a;i++)
  {Serial.println(rec[i]);}
  button=0;
    }
    //---------------------------

//This is the function  that measures the distances using the ultrassonic sensors.
//As the ending of  the maze is always in the right corner, the function will give priority the right  . It will only start measuring the front distance
//if the right sensor detects  an obstacle (values below 20cm). The same goes for the left sensor, it will only  start measuring if the right and front sensors detect an obstacle.
//This type  of functioning prevents the car to waste time in measuring distances that aren't  be necessary.
//The function pings() returns a value, called dir, between 1 and  4. 1 - No obstacle on the right, 2 - Obstacle on the right, 3 - Obstacles in front  and right and 4 - Obstacles in all directions.
    
int pings(){
const  int D = 20; 
  
    dirdist = ping(pinge1);
    Serial.print("distance  at right(->) = ");
    Serial.println(dirdist);
    if(dirdist>D){dir=1;}
    else{
    fdist = ping(pinge2);
    Serial.print("distance at front(^)  = ");
    Serial.println(fdist);
    if(fdist>D){dir=2;}
    else{ 
    esqdist = ping(pinge3);
    Serial.print("distance at left(<-) = ");
    Serial.println(esqdist);
    if(esqdist>D){dir=3;}
    else {dir=4;}
    }
    }
return dir;
}






    //---------------------------

void  loop() {  
MazeMaster_FSM();
}

MazeMaster.ino

c_cpp

1//Authors: Gonçalo Azinheira and João Castro, students at University of
2  the Algarve(Ualg);
3//Project: Mazemaster3000;
4//Subject: A finite state machine-powered
5  maze solver. The starting point of the maze must be in one of the left corners and
6  the ending must be in one of the right corners.
7
8//----------------------------------------------------------------------------
9
10const
11  int pingt = 7; //the same trigger is used by all sensors
12const int pinge2 = 6;
13  //front ultrassonic sensor
14const int pinge1 = 8; //right ultrassonic sensor
15const
16  int pinge3 = 9; //left ultrassonic sensor
17int rec[50];
18int a;
19int In1 =
20  A5; //right
21int In2 = A4; //right
22int In3 = A3; //left
23int In4 = A2; //left
24int
25  EnA = 10; //right
26int EnB = 11; //left
27int dir;
28int esqdist;
29int dirdist;
30int
31  fdist;
32boolean button=0;
33
34void B_ISR(){ //The button is activated by an
35  Interrupt System Routine
36    button=1;
37 }  
38
39void setup() {
40Serial.begin(9600);
41//button=0;
42pinMode(button,
43  INPUT);
44attachInterrupt(0,B_ISR,RISING);
45
46pinMode(pingt, OUTPUT);
47pinMode(pinge1,
48  INPUT);
49pinMode(pinge2, INPUT);
50pinMode(pinge3, INPUT);
51a=0;
52pinMode(In1,
53  OUTPUT);
54pinMode(In2, OUTPUT);
55pinMode(In3, OUTPUT);
56pinMode(In4, OUTPUT);
57pinMode(EnA,
58  OUTPUT);
59pinMode(EnB, OUTPUT);
60pinMode(LED_BUILTIN, OUTPUT);
61analogWrite(EnA,
62  200);
63  analogWrite(EnB, 200);
64}
65
66
67//******************************************
68//---------------------------------------------------
69int
70  ping (int pinge){
71    long duration, cm;
72    int x,xmed;
73    int dela=25;
74
75    int MAX = 1500;
76    int num = 0;
77    int sum=0;
78
79    for(int i=0;i<=5;i++){
80
81  digitalWrite(pingt, LOW);
82  delayMicroseconds(2);
83  digitalWrite(pingt,
84  HIGH);
85  delayMicroseconds(5);
86  digitalWrite(pingt, LOW);
87  duration =
88  pulseIn(pinge, HIGH);
89  x = microsecondsToCentimeters(duration);
90  if(x<MAX){
91
92    num ++;
93  }
94  else{x=0;}
95  sum+=x;
96  
97  delay(dela);
98  }
99
100
101  xmed=sum/num;
102  return xmed;
103  } 
104
105 
106
107  long microsecondsToCentimeters(long
108  microseconds) {
109  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
110
111  // The ping travels out and back, so to find the distance of the object we
112
113  // take half of the distance travelled.
114  return microseconds / 29 / 2;
115}
116
117//-----------------------------------------------
118//**********************************
119//Each
120  funcion uses different values in the analogWrite and delay. That's because the motors
121  aren't exactly the same and the battery will slightly lose power.
122
123
124void
125  walk(int wtime) //Makes the cars go straight
126{
127  analogWrite(EnA, 173); 
128
129  analogWrite(EnB, 171);
130  digitalWrite(In1, HIGH);
131  digitalWrite(In2, LOW);
132
133  digitalWrite(In3, HIGH);
134  digitalWrite(In4, LOW);
135  delay(wtime*1.8);
136
137  digitalWrite(In1, LOW);
138  digitalWrite(In2, LOW);
139  digitalWrite(In3, LOW);
140
141  digitalWrite(In4, LOW);
142  delay(5);
143}
144
145
146
147  void turnR(int wtime)
148  //Makes the car turn right and then go straight
149  {
150  analogWrite(EnA, 192);
151
152  analogWrite(EnB, 172);
153  digitalWrite(In1, LOW);
154  digitalWrite(In2, HIGH);
155
156  digitalWrite(In3, HIGH);
157  digitalWrite(In4, LOW);
158  delay(wtime-141);
159
160  analogWrite(EnA, 170);
161  analogWrite(EnB, 170);
162  digitalWrite(In1, HIGH);
163
164  digitalWrite(In2, LOW);
165  digitalWrite(In3, HIGH);
166  digitalWrite(In4, LOW);
167
168  delay(wtime*2+50);
169  digitalWrite(In1, LOW);
170  digitalWrite(In2, LOW);
171
172  digitalWrite(In3, LOW);
173  digitalWrite(In4, LOW);
174  delay(5);
175    }
176
177
178  void turnL(int wtime) //Makes the car turn left and then go straight
179  {
180
181  analogWrite(EnA, 170);
182  analogWrite(EnB, 170);
183  digitalWrite(In1, HIGH);
184
185  digitalWrite(In2, LOW);
186  digitalWrite(In3, LOW);
187  digitalWrite(In4, HIGH);
188
189  delay(wtime-130);
190  analogWrite(EnA, 170);
191  analogWrite(EnB, 170);
192
193  digitalWrite(In1, HIGH);
194  digitalWrite(In2, LOW);
195  digitalWrite(In3, HIGH);
196
197  digitalWrite(In4, LOW);
198  delay(wtime*2+30);
199  digitalWrite(In1, LOW);
200
201  digitalWrite(In2, LOW);
202  digitalWrite(In3, LOW);
203  digitalWrite(In4, LOW);
204
205  delay(5);
206    }
207
208    void back(int wtime) //Makes the car turn around
209  180. This happens when the sensor detect obstacles in all directions.
210  {
211
212  analogWrite(EnA, 170);
213  analogWrite(EnB, 170);
214  digitalWrite(In1, LOW);
215
216  digitalWrite(In2, HIGH);
217  digitalWrite(In3, HIGH);
218  digitalWrite(In4,
219  LOW);
220  delay(wtime+220);
221  digitalWrite(In1, LOW);
222  digitalWrite(In2,
223  LOW);
224  digitalWrite(In3, LOW);
225  digitalWrite(In4, LOW);
226  delay(5);
227
228  }
229
230//**********************************
231//-----------------------------------------------
232
233//This
234  is the implementation of the finit state machine shown in the picture. It starts
235  always in state -1 and stays there until the button is pressed. After that, it will
236  go to state 0
237//and measures the distances using the ultrassonic sensors using
238  the pings() function. According to what it measured, the finite state machine will
239  go to the corresponding state using the
240//dir value (the meaning of this variable
241  is explained below in the pings() function). The states 1 to 4 are responsible for
242  making the car turn right, go straight, turn left or turn around respectively.
243//At
244  each of those states, the first thing that is done is record a value in the array
245  rec []. This value is equal to the the present state of the machine. For example,
246  if the car is going
247//straight ahead, it is in state 2 and the value recored
248  in the array will be also 2. In other words, the array stores the actions done by
249  the car to be written in the function Recb().
250//The cars reaches to the end if
251  it does 3 lefts in a row and to do so, it needs to go to state 3, state 8 and 5
252  consecutively. So state 8 represents the 2nd left and state 5 represents the
253//3rd
254  left. After reaching to the end of the maze, it goes from state 5 to 6. By this
255  point, the finite state machine will be always moving from state 6 to 7 and vice-versa.
256//State
257  6 turns on the arduino's builtin LED and state 7 turns it off. Regardless of if
258  it is in 6 or 7, it waits for the button to be pressed to print the log.
259
260void
261  MazeMaster_FSM(){
262static int state=-1; //The initial state is always -1 until
263  the button is pressed.
264const int D = 20;  //This is the reference distance, in
265  cm, for the car to consider the obstacles. If the sensor detects something below
266  this distance, it will consider as an obstacle.
267const int wtime = 500;  //Time
268  used on each action(right, straight, left or turn around).
269
270 
271
272  switch(state){
273
274
275    case -1:
276    if(button==1)
277    {button=0;
278    state=0;}
279    break;
280
281    
282    case 0:
283   dir=pings();
284   state=dir;
285    break;
286
287    case
288  1:
289    Serial.print("state = ");
290    Serial.println(state);
291    rec[a]=1;
292
293    a++;
294    turnR(wtime);
295    
296    dir=pings();
297   state=dir;
298    break;
299
300    
301    case 2:
302    Serial.print("state = ");
303    Serial.println(state);
304
305    rec[a]=2;
306    a++;
307    Serial.print("a2 = ");
308    Serial.println(a);
309
310    walk(wtime);
311    
312    dir=pings();
313   state=dir;
314    break;
315
316    
317    case 3:
318    Serial.print("state = ");
319    Serial.println(state);
320
321    rec[a]=3;
322    a++;
323    turnL(wtime);
324    
325    dir=pings();
326    if(dir==3)state=8;
327
328    else state=dir;
329    break;
330
331    case 4:
332    Serial.print("state
333  = ");
334    Serial.println(state);
335    rec[a]=4;
336    a++;
337    Serial.print("a4
338  = ");
339    Serial.println(a);
340    back(wtime);
341
342
343    dir=pings();
344
345    state=dir;
346    break;
347
348    case 5:
349    Serial.print("state = ");
350
351    Serial.println(state);
352    rec[a]=3;
353    a++;
354    turnL(wtime);
355
356    Serial.println("I HAVE ARRIVED! ");
357    rec[a]=5;
358    state=6;
359    break;
360
361    
362     case 6:
363     digitalWrite(LED_BUILTIN,HIGH);
364    if(button==1)
365
366    {Recb();
367    button==0;}
368     delay(500);
369    state=7;
370     break;
371
372     
373     case 7:
374     digitalWrite(LED_BUILTIN,LOW);
375    
376    if(button==1)
377
378    {Recb();
379    button==0;}
380    delay(500);
381    state=6;
382     break;
383
384
385     case 8:
386    Serial.print("state = ");
387    Serial.println(state);
388
389    rec[a]=3;
390    a++;
391    turnL(wtime);
392
393    dir=pings();
394    if(dir==3)state=5;
395
396    else state=dir;
397    break;
398
399    
400  }
401
402}
403//------------------------------
404
405void
406  Recb(){
407  Serial.print("start -> ");
408  for(int i=0;i<=a;i++)
409  {Serial.println(rec[i]);}
410
411  button=0;
412    }
413    //---------------------------
414
415//This is the function
416  that measures the distances using the ultrassonic sensors.
417//As the ending of
418  the maze is always in the right corner, the function will give priority the right
419  . It will only start measuring the front distance
420//if the right sensor detects
421  an obstacle (values below 20cm). The same goes for the left sensor, it will only
422  start measuring if the right and front sensors detect an obstacle.
423//This type
424  of functioning prevents the car to waste time in measuring distances that aren't
425  be necessary.
426//The function pings() returns a value, called dir, between 1 and
427  4. 1 - No obstacle on the right, 2 - Obstacle on the right, 3 - Obstacles in front
428  and right and 4 - Obstacles in all directions.
429    
430int pings(){
431const
432  int D = 20; 
433  
434    dirdist = ping(pinge1);
435    Serial.print("distance
436  at right(->) = ");
437    Serial.println(dirdist);
438    if(dirdist>D){dir=1;}
439
440    else{
441    fdist = ping(pinge2);
442    Serial.print("distance at front(^)
443  = ");
444    Serial.println(fdist);
445    if(fdist>D){dir=2;}
446    else{ 
447
448    esqdist = ping(pinge3);
449    Serial.print("distance at left(<-) = ");
450
451    Serial.println(esqdist);
452    if(esqdist>D){dir=3;}
453    else {dir=4;}
454
455    }
456    }
457return dir;
458}
459
460
461
462
463
464
465    //---------------------------
466
467void
468  loop() {  
469MazeMaster_FSM();
470}
471
472  
473

//Authors: Gonçalo Azinheira and João Castro, students at University of
  the Algarve(Ualg);
//Project: Mazemaster3000;
//Subject: A finite state machine-powered
  maze solver. The starting point of the maze must be in one of the left corners and
  the ending must be in one of the right corners.

//----------------------------------------------------------------------------

const
  int pingt = 7; //the same trigger is used by all sensors
const int pinge2 = 6;
  //front ultrassonic sensor
const int pinge1 = 8; //right ultrassonic sensor
const
  int pinge3 = 9; //left ultrassonic sensor
int rec[50];
int a;
int In1 =
  A5; //right
int In2 = A4; //right
int In3 = A3; //left
int In4 = A2; //left
int
  EnA = 10; //right
int EnB = 11; //left
int dir;
int esqdist;
int dirdist;
int
  fdist;
boolean button=0;

void B_ISR(){ //The button is activated by an
  Interrupt System Routine
    button=1;
 }  

void setup() {
Serial.begin(9600);
//button=0;
pinMode(button,
  INPUT);
attachInterrupt(0,B_ISR,RISING);

pinMode(pingt, OUTPUT);
pinMode(pinge1,
  INPUT);
pinMode(pinge2, INPUT);
pinMode(pinge3, INPUT);
a=0;
pinMode(In1,
  OUTPUT);
pinMode(In2, OUTPUT);
pinMode(In3, OUTPUT);
pinMode(In4, OUTPUT);
pinMode(EnA,
  OUTPUT);
pinMode(EnB, OUTPUT);
pinMode(LED_BUILTIN, OUTPUT);
analogWrite(EnA,
  200);
  analogWrite(EnB, 200);
}


//******************************************
//---------------------------------------------------
int
  ping (int pinge){
    long duration, cm;
    int x,xmed;
    int dela=25;

    int MAX = 1500;
    int num = 0;
    int sum=0;

    for(int i=0;i<=5;i++){

  digitalWrite(pingt, LOW);
  delayMicroseconds(2);
  digitalWrite(pingt,
  HIGH);
  delayMicroseconds(5);
  digitalWrite(pingt, LOW);
  duration =
  pulseIn(pinge, HIGH);
  x = microsecondsToCentimeters(duration);
  if(x<MAX){

    num ++;
  }
  else{x=0;}
  sum+=x;
  
  delay(dela);
  }


  xmed=sum/num;
  return xmed;
  } 

 

  long microsecondsToCentimeters(long
  microseconds) {
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.

  // The ping travels out and back, so to find the distance of the object we

  // take half of the distance travelled.
  return microseconds / 29 / 2;
}

//-----------------------------------------------
//**********************************
//Each
  funcion uses different values in the analogWrite and delay. That's because the motors
  aren't exactly the same and the battery will slightly lose power.


void
  walk(int wtime) //Makes the cars go straight
{
  analogWrite(EnA, 173); 

  analogWrite(EnB, 171);
  digitalWrite(In1, HIGH);
  digitalWrite(In2, LOW);

  digitalWrite(In3, HIGH);
  digitalWrite(In4, LOW);
  delay(wtime*1.8);

  digitalWrite(In1, LOW);
  digitalWrite(In2, LOW);
  digitalWrite(In3, LOW);

  digitalWrite(In4, LOW);
  delay(5);
}



  void turnR(int wtime)
  //Makes the car turn right and then go straight
  {
  analogWrite(EnA, 192);

  analogWrite(EnB, 172);
  digitalWrite(In1, LOW);
  digitalWrite(In2, HIGH);

  digitalWrite(In3, HIGH);
  digitalWrite(In4, LOW);
  delay(wtime-141);

  analogWrite(EnA, 170);
  analogWrite(EnB, 170);
  digitalWrite(In1, HIGH);

  digitalWrite(In2, LOW);
  digitalWrite(In3, HIGH);
  digitalWrite(In4, LOW);

  delay(wtime*2+50);
  digitalWrite(In1, LOW);
  digitalWrite(In2, LOW);

  digitalWrite(In3, LOW);
  digitalWrite(In4, LOW);
  delay(5);
    }


  void turnL(int wtime) //Makes the car turn left and then go straight
  {

  analogWrite(EnA, 170);
  analogWrite(EnB, 170);
  digitalWrite(In1, HIGH);

  digitalWrite(In2, LOW);
  digitalWrite(In3, LOW);
  digitalWrite(In4, HIGH);

  delay(wtime-130);
  analogWrite(EnA, 170);
  analogWrite(EnB, 170);

  digitalWrite(In1, HIGH);
  digitalWrite(In2, LOW);
  digitalWrite(In3, HIGH);

  digitalWrite(In4, LOW);
  delay(wtime*2+30);
  digitalWrite(In1, LOW);

  digitalWrite(In2, LOW);
  digitalWrite(In3, LOW);
  digitalWrite(In4, LOW);

  delay(5);
    }

    void back(int wtime) //Makes the car turn around
  180. This happens when the sensor detect obstacles in all directions.
  {

  analogWrite(EnA, 170);
  analogWrite(EnB, 170);
  digitalWrite(In1, LOW);

  digitalWrite(In2, HIGH);
  digitalWrite(In3, HIGH);
  digitalWrite(In4,
  LOW);
  delay(wtime+220);
  digitalWrite(In1, LOW);
  digitalWrite(In2,
  LOW);
  digitalWrite(In3, LOW);
  digitalWrite(In4, LOW);
  delay(5);

  }

//**********************************
//-----------------------------------------------

//This
  is the implementation of the finit state machine shown in the picture. It starts
  always in state -1 and stays there until the button is pressed. After that, it will
  go to state 0
//and measures the distances using the ultrassonic sensors using
  the pings() function. According to what it measured, the finite state machine will
  go to the corresponding state using the
//dir value (the meaning of this variable
  is explained below in the pings() function). The states 1 to 4 are responsible for
  making the car turn right, go straight, turn left or turn around respectively.
//At
  each of those states, the first thing that is done is record a value in the array
  rec []. This value is equal to the the present state of the machine. For example,
  if the car is going
//straight ahead, it is in state 2 and the value recored
  in the array will be also 2. In other words, the array stores the actions done by
  the car to be written in the function Recb().
//The cars reaches to the end if
  it does 3 lefts in a row and to do so, it needs to go to state 3, state 8 and 5
  consecutively. So state 8 represents the 2nd left and state 5 represents the
//3rd
  left. After reaching to the end of the maze, it goes from state 5 to 6. By this
  point, the finite state machine will be always moving from state 6 to 7 and vice-versa.
//State
  6 turns on the arduino's builtin LED and state 7 turns it off. Regardless of if
  it is in 6 or 7, it waits for the button to be pressed to print the log.

void
  MazeMaster_FSM(){
static int state=-1; //The initial state is always -1 until
  the button is pressed.
const int D = 20;  //This is the reference distance, in
  cm, for the car to consider the obstacles. If the sensor detects something below
  this distance, it will consider as an obstacle.
const int wtime = 500;  //Time
  used on each action(right, straight, left or turn around).

 

  switch(state){


    case -1:
    if(button==1)
    {button=0;
    state=0;}
    break;

    
    case 0:
   dir=pings();
   state=dir;
    break;

    case
  1:
    Serial.print("state = ");
    Serial.println(state);
    rec[a]=1;

    a++;
    turnR(wtime);
    
    dir=pings();
   state=dir;
    break;

    
    case 2:
    Serial.print("state = ");
    Serial.println(state);

    rec[a]=2;
    a++;
    Serial.print("a2 = ");
    Serial.println(a);

    walk(wtime);
    
    dir=pings();
   state=dir;
    break;

    
    case 3:
    Serial.print("state = ");
    Serial.println(state);

    rec[a]=3;
    a++;
    turnL(wtime);
    
    dir=pings();
    if(dir==3)state=8;

    else state=dir;
    break;

    case 4:
    Serial.print("state
  = ");
    Serial.println(state);
    rec[a]=4;
    a++;
    Serial.print("a4
  = ");
    Serial.println(a);
    back(wtime);


    dir=pings();

    state=dir;
    break;

    case 5:
    Serial.print("state = ");

    Serial.println(state);
    rec[a]=3;
    a++;
    turnL(wtime);

    Serial.println("I HAVE ARRIVED! ");
    rec[a]=5;
    state=6;
    break;

    
     case 6:
     digitalWrite(LED_BUILTIN,HIGH);
    if(button==1)

    {Recb();
    button==0;}
     delay(500);
    state=7;
     break;

     
     case 7:
     digitalWrite(LED_BUILTIN,LOW);
    
    if(button==1)

    {Recb();
    button==0;}
    delay(500);
    state=6;
     break;


     case 8:
    Serial.print("state = ");
    Serial.println(state);

    rec[a]=3;
    a++;
    turnL(wtime);

    dir=pings();
    if(dir==3)state=5;

    else state=dir;
    break;

    
  }

}
//------------------------------

void
  Recb(){
  Serial.print("start -> ");
  for(int i=0;i<=a;i++)
  {Serial.println(rec[i]);}

  button=0;
    }
    //---------------------------

//This is the function
  that measures the distances using the ultrassonic sensors.
//As the ending of
  the maze is always in the right corner, the function will give priority the right
  . It will only start measuring the front distance
//if the right sensor detects
  an obstacle (values below 20cm). The same goes for the left sensor, it will only
  start measuring if the right and front sensors detect an obstacle.
//This type
  of functioning prevents the car to waste time in measuring distances that aren't
  be necessary.
//The function pings() returns a value, called dir, between 1 and
  4. 1 - No obstacle on the right, 2 - Obstacle on the right, 3 - Obstacles in front
  and right and 4 - Obstacles in all directions.
    
int pings(){
const
  int D = 20; 
  
    dirdist = ping(pinge1);
    Serial.print("distance
  at right(->) = ");
    Serial.println(dirdist);
    if(dirdist>D){dir=1;}

    else{
    fdist = ping(pinge2);
    Serial.print("distance at front(^)
  = ");
    Serial.println(fdist);
    if(fdist>D){dir=2;}
    else{ 

    esqdist = ping(pinge3);
    Serial.print("distance at left(<-) = ");

    Serial.println(esqdist);
    if(esqdist>D){dir=3;}
    else {dir=4;}

    }
    }
return dir;
}






    //---------------------------

void
  loop() {  
MazeMaster_FSM();
}

Downloadable files

Part of the schematics

To finish you should take the L298N, remove the jumpers in EnA and EnB and connect 5V to Arduino Vin, GND to Arduino GND, OUT1 & 2 to each of the poles of motor A(right) and OUT3&4 to each of the poles of motor B(left) EnA and EnB should be on Arduino digital pins 10 and 11 respectively. They MUST be on pwm pins and 10 and 11 have the same frequency, so this shouldn't be tinkered with. In1, 2, 3 and 4 should be on Arduino ANALOG PINS 5, 4, 3 and 2 respectively. WARNING:The capacitor connection to ground MUST be disconnected when uploading or it will NOT upload. Connect BEFORE connecting to the computer after the Arduino's BUILTIN LED starts blinking (after 3 lefts).

Part of the schematics

Comments

Only logged in users can leave comments

mazemaster3000

0 Followers

•

0 Projects

Intro

Components and supplies

Tools and machines

Project description

Code

Downloadable files