Pathfinder

Design of an indicative featured and speed controlled obstacle avoiding robot.

Feb 28, 2018

•

86279 views

•

138 respects

obstacle avoiding robot

Components and supplies

Arduino UNO

USB-A to B Cable

Resistor 330 ohm

Li-Ion Battery 1000mAh

Buzzer

RGB Diffused Common Cathode

Jumper wires (generic)

Ultrasonic Sensor - HC-SR04 (Generic)

Tools and machines

Materia 101

Digilent Screwdriver

Apps and platforms

Fusion 360

Arduino IDE

Project description

Code

Program

arduino

The assembly program, for setting up accordingly this autonomous machine.

1#include <AFMotor.h>  // Add Adafruit Motor Shield for Arduino kit library.
2#include   <Servo.h>    // Add Servo Motor library.
3#define BuzzPIN A0  // Assign PIN A0   as BuzzPIN (Connect Arduino UNO "A0" PIN with Buzzer "+" PIN).
4#define TrigPIN   A1  // Assign PIN A1 as TrigPIN (Connect Arduino UNO "A1" PIN with Ultrasonic   Sonar Sensor "Trig" PIN).
5#define EchoPIN A2  // Assign PIN A2 as EchoPIN (Connect   Arduino UNO "A2" PIN with Ultrasonic Sonar Sensor "Trig" PIN).
6#define LEDBPIN   A3  // Assign PIN A3 as LEDBPIN (Connect Arduino UNO "A3" PIN with RGB Diffused   Common Cathode "LEDB" PIN).
7#define LEDGPIN A4  // Assign PIN A4 as LEDGPIN   (Connect Arduino UNO "A4" PIN with RGB Diffused Common Cathode "LEDG" PIN).
8#define   LEDRPIN A5  // Assign PIN A5 as LEDRPIN (Connect Arduino UNO "A5" PIN with RGB   Diffused Common Cathode "LEDR" PIN).
9#define DCMROFF 25  // This sets Offset   to allow differences between the two DC traction Motors.
10AF_DCMotor M1 (1, MOTOR12_64KHZ);   // Create DCMotor #1 using M1 output, Set to 64kHz PWM frequency.
11AF_DCMotor   M2 (2, MOTOR12_64KHZ); // Create DCMotor #2 using M2 output, Set to 64kHz PWM frequency.
12Servo   SER1; // Create Servo object to control Servo.
13int Search (void) {                   //   Integer type variable declaration.
14  float Duration = 0.0;               // Float   type variable declaration.
15  float CM = 0.0;                     // Float type   variable declaration.
16  digitalWrite (TrigPIN, LOW);        // TrigPIN output   as 0V (Logic low level).
17  delayMicroseconds (2);              // Delay for 2us,   Send 10 us high pulse to Ultrasonic Sonar Sensor "TrigPIN".
18  digitalWrite   (TrigPIN, HIGH);       // TrigPIN output as 5V (Logic high level).
19  delayMicroseconds   (10);             // Delay for 10us.
20  digitalWrite (TrigPIN, LOW);        //   TrigPIN output as 0V (Logic low level).
21  Duration = pulseIn (EchoPIN, HIGH);   // Start counting time, Upto again EchoPIN back to logic "High Level" and puting   the "Time" into variable called "Duration".
22  CM = (Duration/58.8);               //   Convert Distance into CM.
23  return CM;                          // Return to   CM.
24}
25int RightDistance, LeftDistance;  // Distances on either side.
26float   Distance = 0.00;            // Float type variable declaration.
27void setup ()   {               // Setup loop.
28  pinMode (BuzzPIN, OUTPUT);  // Declare BuzzPIN   as "Output PIN".
29  pinMode (TrigPIN, OUTPUT);  // Declare TrigPIN as "Output   PIN".
30  pinMode (EchoPIN, INPUT);   // Declare EchoPIN as "Output PIN".
31   pinMode (LEDBPIN, OUTPUT);  // Declare LEDBPIN as "Output PIN".
32  pinMode   (LEDGPIN, OUTPUT);  // Declare LEDGPIN as "Output PIN".
33  pinMode (LEDRPIN,   OUTPUT);  // Declare LEDRPIN as "Output PIN".
34  SER1.attach (10);           //   Attaches the Servo on pin 10 (SER1 on the Adafruit Motor Shield for Arduino kit   to the Servo object).
35}
36void loop () {                                    //   Main loop.
37  SER1.write (80);                                // Tells the Servo   to position at 80 degrees (Facing forward).
38  delay (100);                                    //   Delay for 0.1s.
39  Distance = Search ();                           // Measuring   the Distance in CM.
40  if (Distance < 30) {                            // If obstacle   found in 30cm.
41    digitalWrite (BuzzPIN, HIGH);                 // BuzzPIN output   as 5V (Logic high level).
42    digitalWrite (LEDBPIN, LOW);                  //   LEDBPIN output as 0V (Logic low level).
43    digitalWrite (LEDGPIN, LOW);                  //   LEDGPIN output as 0V (Logic low level).
44    digitalWrite (LEDRPIN, HIGH);                 //   LEDRPIN output as 5V (Logic high level).
45    M1.setSpeed (100);                            //   Speed down.
46    M2.setSpeed (100);                            // Speed down.
47     ChangePath ();                                // If forward is blocked Change   direction.
48  }
49  else if ((Distance >= 30) && (Distance < 60)) { // If obstacle   found between 30cm to 60cm.
50    digitalWrite (BuzzPIN, LOW);                  //   BuzzPIN output as 0V (Logic low level).
51    digitalWrite (LEDBPIN, HIGH);                 //   LEDBPIN output as 5V (Logic high level).
52    digitalWrite (LEDGPIN, LOW);                  //   LEDGPIN output as 0V (Logic low level).
53    digitalWrite (LEDRPIN, LOW);                  //   LEDRPIN output as 0V (Logic low level).
54    M1.setSpeed (150);                            //   Speed increase slightly.
55    M2.setSpeed (150);                            //   Speed increase slightly.
56    Forward ();                                   //   Robot move to Forward direction.
57  }
58  else if ((Distance >= 60) && (Distance   < 90)) { // If obstacle found between 60cm to 90cm.
59    digitalWrite (BuzzPIN,   LOW);                  // BuzzPIN output as 0V (Logic low level).
60    digitalWrite   (LEDBPIN, LOW);                  // LEDBPIN output as 0V (Logic low level).
61     digitalWrite (LEDGPIN, HIGH);                 // LEDGPIN output as 5V (Logic   high level).
62    digitalWrite (LEDRPIN, LOW);                  // LEDRPIN output   as 0V (Logic low level).
63    M1.setSpeed (200);                            //   Speed up.
64    M2.setSpeed (200);                            // Speed up.
65     Forward ();                                   // Robot move to Forward direction.
66   }
67  else {                                          // If obstacle cannot be   found in 90cm.
68    digitalWrite (BuzzPIN, LOW);                  // BuzzPIN output   as 0V (Logic low level).
69    digitalWrite (LEDBPIN, HIGH);                 //   LEDBPIN output as 5V (Logic high level).
70    digitalWrite (LEDGPIN, HIGH);                 //   LEDGPIN output as 5V (Logic high level).
71    digitalWrite (LEDRPIN, HIGH);                 //   LEDRPIN output as 5V (Logic high level).
72    M1.setSpeed (250);                            //   Speed increase fully.
73    M2.setSpeed (250);                            // Speed   increase fully.
74    Forward ();                                   // Robot move   to Forward direction.
75  }
76}
77void ChangePath () {         // Path Change   loop.
78  Stop ();                   // Robot Stop.
79  Backward ();               //   Robot run Backward direction.
80  Stop ();                   // Robot Stop.
81   SER1.write (12);           // Check Distance to the Right.
82  delay (500);               //   Delay for 0.5s.
83  RightDistance = Search (); // Set Right Distance.
84  delay   (500);               // Delay for 0.5s.
85  SER1.write (160);          // Check   Distance to the Left.
86  delay (1000);              // Delay for 1s.
87  LeftDistance   = Search ();  // Set Left Distance.
88  delay (500);               // Delay for   0.5s.
89  SER1.write (80);           // Return to center.
90  delay (500);               //   Delay for 0.5s.
91  CompareDistance ();        // Find the longest distance.
92}
93void   CompareDistance () {                   // Distance Compare loop.
94  if (RightDistance   > LeftDistance) {       // If Right is less obstructed.
95     TurnRight ();                          //   Robot Turn into Right direction.
96    }
97  else if (LeftDistance > RightDistance)   {  // If Left is less obstructed.
98      TurnLeft ();                          //   Robot Turn into Left direction.
99    }
100  else {                                    //   If both are equally obstructed.
101      TurnAround ();                        //   Robot Turn Around.
102    }
103}
104void Forward () {   // Forward loop.
105  M1.run   (FORWARD); // Turn DCMotor #1 to Forward.
106  M2.run (FORWARD); // Turn DCMotor   #1 to Forward.
107}
108void Backward () {    // Backward loop.
109  M1.run (BACKWARD);   // Turn DCMotor #1 to Backward.
110  M2.run (BACKWARD);  // Turn DCMotor #2 to   Backward.
111  delay (500);       // Delay for 1s.
112}
113void TurnRight () {           //   Right Turn loop.
114  M1.run (BACKWARD);          // Turn DCMotor #1 to Backward.
115   M2.run (FORWARD);           // Turn DCMotor #2 to Forward.
116  M1.setSpeed (100+DCMROFF);   // Calibrate the Speed of DCMotor #1.
117  delay (300);                // Delay   for 0.7s.
118}
119void TurnLeft () {            // Left Turn loop.
120  M1.run (FORWARD);            // Turn DCMotor #1 to Forward.
121  M2.run (BACKWARD);          // Turn   DCMotor #2 to Backward.
122  M2.setSpeed (100+DCMROFF);  // Calibrate the Speed   of DCMotor #2.
123  delay (300);                // Delay for 0.7s.
124}
125void   TurnAround () {          // Trun Around loop.
126  M1.run (FORWARD);           //   Turn DCMotor #1 to Forward.
127  M2.run (BACKWARD);          // Turn DCMotor #2   to Backward.
128  M2.setSpeed (100+DCMROFF);  // Calibrate the Speed of DCMotor   #2.
129  delay (700);               // Delay for 2.1s.
130}
131void Stop () {      //   Stop loop.
132  M1.run (RELEASE); // Release DCMotor #1.
133  M2.run (RELEASE);   // Release DCMotor #2.
134  delay (100);      // Delay for 0.1s.
135}

#include <AFMotor.h>  // Add Adafruit Motor Shield for Arduino kit library.
#include   <Servo.h>    // Add Servo Motor library.
#define BuzzPIN A0  // Assign PIN A0   as BuzzPIN (Connect Arduino UNO "A0" PIN with Buzzer "+" PIN).
#define TrigPIN   A1  // Assign PIN A1 as TrigPIN (Connect Arduino UNO "A1" PIN with Ultrasonic   Sonar Sensor "Trig" PIN).
#define EchoPIN A2  // Assign PIN A2 as EchoPIN (Connect   Arduino UNO "A2" PIN with Ultrasonic Sonar Sensor "Trig" PIN).
#define LEDBPIN   A3  // Assign PIN A3 as LEDBPIN (Connect Arduino UNO "A3" PIN with RGB Diffused   Common Cathode "LEDB" PIN).
#define LEDGPIN A4  // Assign PIN A4 as LEDGPIN   (Connect Arduino UNO "A4" PIN with RGB Diffused Common Cathode "LEDG" PIN).
#define   LEDRPIN A5  // Assign PIN A5 as LEDRPIN (Connect Arduino UNO "A5" PIN with RGB   Diffused Common Cathode "LEDR" PIN).
#define DCMROFF 25  // This sets Offset   to allow differences between the two DC traction Motors.
AF_DCMotor M1 (1, MOTOR12_64KHZ);   // Create DCMotor #1 using M1 output, Set to 64kHz PWM frequency.
AF_DCMotor   M2 (2, MOTOR12_64KHZ); // Create DCMotor #2 using M2 output, Set to 64kHz PWM frequency.
Servo   SER1; // Create Servo object to control Servo.
int Search (void) {                   //   Integer type variable declaration.
  float Duration = 0.0;               // Float   type variable declaration.
  float CM = 0.0;                     // Float type   variable declaration.
  digitalWrite (TrigPIN, LOW);        // TrigPIN output   as 0V (Logic low level).
  delayMicroseconds (2);              // Delay for 2us,   Send 10 us high pulse to Ultrasonic Sonar Sensor "TrigPIN".
  digitalWrite   (TrigPIN, HIGH);       // TrigPIN output as 5V (Logic high level).
  delayMicroseconds   (10);             // Delay for 10us.
  digitalWrite (TrigPIN, LOW);        //   TrigPIN output as 0V (Logic low level).
  Duration = pulseIn (EchoPIN, HIGH);   // Start counting time, Upto again EchoPIN back to logic "High Level" and puting   the "Time" into variable called "Duration".
  CM = (Duration/58.8);               //   Convert Distance into CM.
  return CM;                          // Return to   CM.
}
int RightDistance, LeftDistance;  // Distances on either side.
float   Distance = 0.00;            // Float type variable declaration.
void setup ()   {               // Setup loop.
  pinMode (BuzzPIN, OUTPUT);  // Declare BuzzPIN   as "Output PIN".
  pinMode (TrigPIN, OUTPUT);  // Declare TrigPIN as "Output   PIN".
  pinMode (EchoPIN, INPUT);   // Declare EchoPIN as "Output PIN".
   pinMode (LEDBPIN, OUTPUT);  // Declare LEDBPIN as "Output PIN".
  pinMode   (LEDGPIN, OUTPUT);  // Declare LEDGPIN as "Output PIN".
  pinMode (LEDRPIN,   OUTPUT);  // Declare LEDRPIN as "Output PIN".
  SER1.attach (10);           //   Attaches the Servo on pin 10 (SER1 on the Adafruit Motor Shield for Arduino kit   to the Servo object).
}
void loop () {                                    //   Main loop.
  SER1.write (80);                                // Tells the Servo   to position at 80 degrees (Facing forward).
  delay (100);                                    //   Delay for 0.1s.
  Distance = Search ();                           // Measuring   the Distance in CM.
  if (Distance < 30) {                            // If obstacle   found in 30cm.
    digitalWrite (BuzzPIN, HIGH);                 // BuzzPIN output   as 5V (Logic high level).
    digitalWrite (LEDBPIN, LOW);                  //   LEDBPIN output as 0V (Logic low level).
    digitalWrite (LEDGPIN, LOW);                  //   LEDGPIN output as 0V (Logic low level).
    digitalWrite (LEDRPIN, HIGH);                 //   LEDRPIN output as 5V (Logic high level).
    M1.setSpeed (100);                            //   Speed down.
    M2.setSpeed (100);                            // Speed down.
     ChangePath ();                                // If forward is blocked Change   direction.
  }
  else if ((Distance >= 30) && (Distance < 60)) { // If obstacle   found between 30cm to 60cm.
    digitalWrite (BuzzPIN, LOW);                  //   BuzzPIN output as 0V (Logic low level).
    digitalWrite (LEDBPIN, HIGH);                 //   LEDBPIN output as 5V (Logic high level).
    digitalWrite (LEDGPIN, LOW);                  //   LEDGPIN output as 0V (Logic low level).
    digitalWrite (LEDRPIN, LOW);                  //   LEDRPIN output as 0V (Logic low level).
    M1.setSpeed (150);                            //   Speed increase slightly.
    M2.setSpeed (150);                            //   Speed increase slightly.
    Forward ();                                   //   Robot move to Forward direction.
  }
  else if ((Distance >= 60) && (Distance   < 90)) { // If obstacle found between 60cm to 90cm.
    digitalWrite (BuzzPIN,   LOW);                  // BuzzPIN output as 0V (Logic low level).
    digitalWrite   (LEDBPIN, LOW);                  // LEDBPIN output as 0V (Logic low level).
     digitalWrite (LEDGPIN, HIGH);                 // LEDGPIN output as 5V (Logic   high level).
    digitalWrite (LEDRPIN, LOW);                  // LEDRPIN output   as 0V (Logic low level).
    M1.setSpeed (200);                            //   Speed up.
    M2.setSpeed (200);                            // Speed up.
     Forward ();                                   // Robot move to Forward direction.
   }
  else {                                          // If obstacle cannot be   found in 90cm.
    digitalWrite (BuzzPIN, LOW);                  // BuzzPIN output   as 0V (Logic low level).
    digitalWrite (LEDBPIN, HIGH);                 //   LEDBPIN output as 5V (Logic high level).
    digitalWrite (LEDGPIN, HIGH);                 //   LEDGPIN output as 5V (Logic high level).
    digitalWrite (LEDRPIN, HIGH);                 //   LEDRPIN output as 5V (Logic high level).
    M1.setSpeed (250);                            //   Speed increase fully.
    M2.setSpeed (250);                            // Speed   increase fully.
    Forward ();                                   // Robot move   to Forward direction.
  }
}
void ChangePath () {         // Path Change   loop.
  Stop ();                   // Robot Stop.
  Backward ();               //   Robot run Backward direction.
  Stop ();                   // Robot Stop.
   SER1.write (12);           // Check Distance to the Right.
  delay (500);               //   Delay for 0.5s.
  RightDistance = Search (); // Set Right Distance.
  delay   (500);               // Delay for 0.5s.
  SER1.write (160);          // Check   Distance to the Left.
  delay (1000);              // Delay for 1s.
  LeftDistance   = Search ();  // Set Left Distance.
  delay (500);               // Delay for   0.5s.
  SER1.write (80);           // Return to center.
  delay (500);               //   Delay for 0.5s.
  CompareDistance ();        // Find the longest distance.
}
void   CompareDistance () {                   // Distance Compare loop.
  if (RightDistance   > LeftDistance) {       // If Right is less obstructed.
     TurnRight ();                          //   Robot Turn into Right direction.
    }
  else if (LeftDistance > RightDistance)   {  // If Left is less obstructed.
      TurnLeft ();                          //   Robot Turn into Left direction.
    }
  else {                                    //   If both are equally obstructed.
      TurnAround ();                        //   Robot Turn Around.
    }
}
void Forward () {   // Forward loop.
  M1.run   (FORWARD); // Turn DCMotor #1 to Forward.
  M2.run (FORWARD); // Turn DCMotor   #1 to Forward.
}
void Backward () {    // Backward loop.
  M1.run (BACKWARD);   // Turn DCMotor #1 to Backward.
  M2.run (BACKWARD);  // Turn DCMotor #2 to   Backward.
  delay (500);       // Delay for 1s.
}
void TurnRight () {           //   Right Turn loop.
  M1.run (BACKWARD);          // Turn DCMotor #1 to Backward.
   M2.run (FORWARD);           // Turn DCMotor #2 to Forward.
  M1.setSpeed (100+DCMROFF);   // Calibrate the Speed of DCMotor #1.
  delay (300);                // Delay   for 0.7s.
}
void TurnLeft () {            // Left Turn loop.
  M1.run (FORWARD);            // Turn DCMotor #1 to Forward.
  M2.run (BACKWARD);          // Turn   DCMotor #2 to Backward.
  M2.setSpeed (100+DCMROFF);  // Calibrate the Speed   of DCMotor #2.
  delay (300);                // Delay for 0.7s.
}
void   TurnAround () {          // Trun Around loop.
  M1.run (FORWARD);           //   Turn DCMotor #1 to Forward.
  M2.run (BACKWARD);          // Turn DCMotor #2   to Backward.
  M2.setSpeed (100+DCMROFF);  // Calibrate the Speed of DCMotor   #2.
  delay (700);               // Delay for 2.1s.
}
void Stop () {      //   Stop loop.
  M1.run (RELEASE); // Release DCMotor #1.
  M2.run (RELEASE);   // Release DCMotor #2.
  delay (100);      // Delay for 0.1s.
}

Program

arduino

The assembly program, for setting up accordingly this autonomous machine.

1#include <AFMotor.h>  // Add Adafruit Motor Shield for Arduino kit library.
2#include  <Servo.h>    // Add Servo Motor library.
3#define BuzzPIN A0  // Assign PIN A0  as BuzzPIN (Connect Arduino UNO "A0" PIN with Buzzer "+" PIN).
4#define TrigPIN  A1  // Assign PIN A1 as TrigPIN (Connect Arduino UNO "A1" PIN with Ultrasonic  Sonar Sensor "Trig" PIN).
5#define EchoPIN A2  // Assign PIN A2 as EchoPIN (Connect  Arduino UNO "A2" PIN with Ultrasonic Sonar Sensor "Trig" PIN).
6#define LEDBPIN  A3  // Assign PIN A3 as LEDBPIN (Connect Arduino UNO "A3" PIN with RGB Diffused  Common Cathode "LEDB" PIN).
7#define LEDGPIN A4  // Assign PIN A4 as LEDGPIN  (Connect Arduino UNO "A4" PIN with RGB Diffused Common Cathode "LEDG" PIN).
8#define  LEDRPIN A5  // Assign PIN A5 as LEDRPIN (Connect Arduino UNO "A5" PIN with RGB  Diffused Common Cathode "LEDR" PIN).
9#define DCMROFF 25  // This sets Offset  to allow differences between the two DC traction Motors.
10AF_DCMotor M1 (1, MOTOR12_64KHZ);  // Create DCMotor #1 using M1 output, Set to 64kHz PWM frequency.
11AF_DCMotor  M2 (2, MOTOR12_64KHZ); // Create DCMotor #2 using M2 output, Set to 64kHz PWM frequency.
12Servo  SER1; // Create Servo object to control Servo.
13int Search (void) {                   //  Integer type variable declaration.
14  float Duration = 0.0;               // Float  type variable declaration.
15  float CM = 0.0;                     // Float type  variable declaration.
16  digitalWrite (TrigPIN, LOW);        // TrigPIN output  as 0V (Logic low level).
17  delayMicroseconds (2);              // Delay for 2us,  Send 10 us high pulse to Ultrasonic Sonar Sensor "TrigPIN".
18  digitalWrite  (TrigPIN, HIGH);       // TrigPIN output as 5V (Logic high level).
19  delayMicroseconds  (10);             // Delay for 10us.
20  digitalWrite (TrigPIN, LOW);        //  TrigPIN output as 0V (Logic low level).
21  Duration = pulseIn (EchoPIN, HIGH);  // Start counting time, Upto again EchoPIN back to logic "High Level" and puting  the "Time" into variable called "Duration".
22  CM = (Duration/58.8);               //  Convert Distance into CM.
23  return CM;                          // Return to  CM.
24}
25int RightDistance, LeftDistance;  // Distances on either side.
26float  Distance = 0.00;            // Float type variable declaration.
27void setup ()  {               // Setup loop.
28  pinMode (BuzzPIN, OUTPUT);  // Declare BuzzPIN  as "Output PIN".
29  pinMode (TrigPIN, OUTPUT);  // Declare TrigPIN as "Output  PIN".
30  pinMode (EchoPIN, INPUT);   // Declare EchoPIN as "Output PIN".
31  pinMode (LEDBPIN, OUTPUT);  // Declare LEDBPIN as "Output PIN".
32  pinMode  (LEDGPIN, OUTPUT);  // Declare LEDGPIN as "Output PIN".
33  pinMode (LEDRPIN,  OUTPUT);  // Declare LEDRPIN as "Output PIN".
34  SER1.attach (10);           //  Attaches the Servo on pin 10 (SER1 on the Adafruit Motor Shield for Arduino kit  to the Servo object).
35}
36void loop () {                                    //  Main loop.
37  SER1.write (80);                                // Tells the Servo  to position at 80 degrees (Facing forward).
38  delay (100);                                    //  Delay for 0.1s.
39  Distance = Search ();                           // Measuring  the Distance in CM.
40  if (Distance < 30) {                            // If obstacle  found in 30cm.
41    digitalWrite (BuzzPIN, HIGH);                 // BuzzPIN output  as 5V (Logic high level).
42    digitalWrite (LEDBPIN, LOW);                  //  LEDBPIN output as 0V (Logic low level).
43    digitalWrite (LEDGPIN, LOW);                  //  LEDGPIN output as 0V (Logic low level).
44    digitalWrite (LEDRPIN, HIGH);                 //  LEDRPIN output as 5V (Logic high level).
45    M1.setSpeed (100);                            //  Speed down.
46    M2.setSpeed (100);                            // Speed down.
47    ChangePath ();                                // If forward is blocked Change  direction.
48  }
49  else if ((Distance >= 30) && (Distance < 60)) { // If obstacle  found between 30cm to 60cm.
50    digitalWrite (BuzzPIN, LOW);                  //  BuzzPIN output as 0V (Logic low level).
51    digitalWrite (LEDBPIN, HIGH);                 //  LEDBPIN output as 5V (Logic high level).
52    digitalWrite (LEDGPIN, LOW);                  //  LEDGPIN output as 0V (Logic low level).
53    digitalWrite (LEDRPIN, LOW);                  //  LEDRPIN output as 0V (Logic low level).
54    M1.setSpeed (150);                            //  Speed increase slightly.
55    M2.setSpeed (150);                            //  Speed increase slightly.
56    Forward ();                                   //  Robot move to Forward direction.
57  }
58  else if ((Distance >= 60) && (Distance  < 90)) { // If obstacle found between 60cm to 90cm.
59    digitalWrite (BuzzPIN,  LOW);                  // BuzzPIN output as 0V (Logic low level).
60    digitalWrite  (LEDBPIN, LOW);                  // LEDBPIN output as 0V (Logic low level).
61    digitalWrite (LEDGPIN, HIGH);                 // LEDGPIN output as 5V (Logic  high level).
62    digitalWrite (LEDRPIN, LOW);                  // LEDRPIN output  as 0V (Logic low level).
63    M1.setSpeed (200);                            //  Speed up.
64    M2.setSpeed (200);                            // Speed up.
65    Forward ();                                   // Robot move to Forward direction.
66  }
67  else {                                          // If obstacle cannot be  found in 90cm.
68    digitalWrite (BuzzPIN, LOW);                  // BuzzPIN output  as 0V (Logic low level).
69    digitalWrite (LEDBPIN, HIGH);                 //  LEDBPIN output as 5V (Logic high level).
70    digitalWrite (LEDGPIN, HIGH);                 //  LEDGPIN output as 5V (Logic high level).
71    digitalWrite (LEDRPIN, HIGH);                 //  LEDRPIN output as 5V (Logic high level).
72    M1.setSpeed (250);                            //  Speed increase fully.
73    M2.setSpeed (250);                            // Speed  increase fully.
74    Forward ();                                   // Robot move  to Forward direction.
75  }
76}
77void ChangePath () {         // Path Change  loop.
78  Stop ();                   // Robot Stop.
79  Backward ();               //  Robot run Backward direction.
80  Stop ();                   // Robot Stop.
81  SER1.write (12);           // Check Distance to the Right.
82  delay (500);               //  Delay for 0.5s.
83  RightDistance = Search (); // Set Right Distance.
84  delay  (500);               // Delay for 0.5s.
85  SER1.write (160);          // Check  Distance to the Left.
86  delay (1000);              // Delay for 1s.
87  LeftDistance  = Search ();  // Set Left Distance.
88  delay (500);               // Delay for  0.5s.
89  SER1.write (80);           // Return to center.
90  delay (500);               //  Delay for 0.5s.
91  CompareDistance ();        // Find the longest distance.
92}
93void  CompareDistance () {                   // Distance Compare loop.
94  if (RightDistance  > LeftDistance) {       // If Right is less obstructed.
95     TurnRight ();                          //  Robot Turn into Right direction.
96    }
97  else if (LeftDistance > RightDistance)  {  // If Left is less obstructed.
98      TurnLeft ();                          //  Robot Turn into Left direction.
99    }
100  else {                                    //  If both are equally obstructed.
101      TurnAround ();                        //  Robot Turn Around.
102    }
103}
104void Forward () {   // Forward loop.
105  M1.run  (FORWARD); // Turn DCMotor #1 to Forward.
106  M2.run (FORWARD); // Turn DCMotor  #1 to Forward.
107}
108void Backward () {    // Backward loop.
109  M1.run (BACKWARD);  // Turn DCMotor #1 to Backward.
110  M2.run (BACKWARD);  // Turn DCMotor #2 to  Backward.
111  delay (500);       // Delay for 1s.
112}
113void TurnRight () {           //  Right Turn loop.
114  M1.run (BACKWARD);          // Turn DCMotor #1 to Backward.
115  M2.run (FORWARD);           // Turn DCMotor #2 to Forward.
116  M1.setSpeed (100+DCMROFF);  // Calibrate the Speed of DCMotor #1.
117  delay (300);                // Delay  for 0.7s.
118}
119void TurnLeft () {            // Left Turn loop.
120  M1.run (FORWARD);           // Turn DCMotor #1 to Forward.
121  M2.run (BACKWARD);          // Turn  DCMotor #2 to Backward.
122  M2.setSpeed (100+DCMROFF);  // Calibrate the Speed  of DCMotor #2.
123  delay (300);                // Delay for 0.7s.
124}
125void  TurnAround () {          // Trun Around loop.
126  M1.run (FORWARD);           //  Turn DCMotor #1 to Forward.
127  M2.run (BACKWARD);          // Turn DCMotor #2  to Backward.
128  M2.setSpeed (100+DCMROFF);  // Calibrate the Speed of DCMotor  #2.
129  delay (700);               // Delay for 2.1s.
130}
131void Stop () {      //  Stop loop.
132  M1.run (RELEASE); // Release DCMotor #1.
133  M2.run (RELEASE);  // Release DCMotor #2.
134  delay (100);      // Delay for 0.1s.
135}

#include <AFMotor.h>  // Add Adafruit Motor Shield for Arduino kit library.
#include  <Servo.h>    // Add Servo Motor library.
#define BuzzPIN A0  // Assign PIN A0  as BuzzPIN (Connect Arduino UNO "A0" PIN with Buzzer "+" PIN).
#define TrigPIN  A1  // Assign PIN A1 as TrigPIN (Connect Arduino UNO "A1" PIN with Ultrasonic  Sonar Sensor "Trig" PIN).
#define EchoPIN A2  // Assign PIN A2 as EchoPIN (Connect  Arduino UNO "A2" PIN with Ultrasonic Sonar Sensor "Trig" PIN).
#define LEDBPIN  A3  // Assign PIN A3 as LEDBPIN (Connect Arduino UNO "A3" PIN with RGB Diffused  Common Cathode "LEDB" PIN).
#define LEDGPIN A4  // Assign PIN A4 as LEDGPIN  (Connect Arduino UNO "A4" PIN with RGB Diffused Common Cathode "LEDG" PIN).
#define  LEDRPIN A5  // Assign PIN A5 as LEDRPIN (Connect Arduino UNO "A5" PIN with RGB  Diffused Common Cathode "LEDR" PIN).
#define DCMROFF 25  // This sets Offset  to allow differences between the two DC traction Motors.
AF_DCMotor M1 (1, MOTOR12_64KHZ);  // Create DCMotor #1 using M1 output, Set to 64kHz PWM frequency.
AF_DCMotor  M2 (2, MOTOR12_64KHZ); // Create DCMotor #2 using M2 output, Set to 64kHz PWM frequency.
Servo  SER1; // Create Servo object to control Servo.
int Search (void) {                   //  Integer type variable declaration.
  float Duration = 0.0;               // Float  type variable declaration.
  float CM = 0.0;                     // Float type  variable declaration.
  digitalWrite (TrigPIN, LOW);        // TrigPIN output  as 0V (Logic low level).
  delayMicroseconds (2);              // Delay for 2us,  Send 10 us high pulse to Ultrasonic Sonar Sensor "TrigPIN".
  digitalWrite  (TrigPIN, HIGH);       // TrigPIN output as 5V (Logic high level).
  delayMicroseconds  (10);             // Delay for 10us.
  digitalWrite (TrigPIN, LOW);        //  TrigPIN output as 0V (Logic low level).
  Duration = pulseIn (EchoPIN, HIGH);  // Start counting time, Upto again EchoPIN back to logic "High Level" and puting  the "Time" into variable called "Duration".
  CM = (Duration/58.8);               //  Convert Distance into CM.
  return CM;                          // Return to  CM.
}
int RightDistance, LeftDistance;  // Distances on either side.
float  Distance = 0.00;            // Float type variable declaration.
void setup ()  {               // Setup loop.
  pinMode (BuzzPIN, OUTPUT);  // Declare BuzzPIN  as "Output PIN".
  pinMode (TrigPIN, OUTPUT);  // Declare TrigPIN as "Output  PIN".
  pinMode (EchoPIN, INPUT);   // Declare EchoPIN as "Output PIN".
  pinMode (LEDBPIN, OUTPUT);  // Declare LEDBPIN as "Output PIN".
  pinMode  (LEDGPIN, OUTPUT);  // Declare LEDGPIN as "Output PIN".
  pinMode (LEDRPIN,  OUTPUT);  // Declare LEDRPIN as "Output PIN".
  SER1.attach (10);           //  Attaches the Servo on pin 10 (SER1 on the Adafruit Motor Shield for Arduino kit  to the Servo object).
}
void loop () {                                    //  Main loop.
  SER1.write (80);                                // Tells the Servo  to position at 80 degrees (Facing forward).
  delay (100);                                    //  Delay for 0.1s.
  Distance = Search ();                           // Measuring  the Distance in CM.
  if (Distance < 30) {                            // If obstacle  found in 30cm.
    digitalWrite (BuzzPIN, HIGH);                 // BuzzPIN output  as 5V (Logic high level).
    digitalWrite (LEDBPIN, LOW);                  //  LEDBPIN output as 0V (Logic low level).
    digitalWrite (LEDGPIN, LOW);                  //  LEDGPIN output as 0V (Logic low level).
    digitalWrite (LEDRPIN, HIGH);                 //  LEDRPIN output as 5V (Logic high level).
    M1.setSpeed (100);                            //  Speed down.
    M2.setSpeed (100);                            // Speed down.
    ChangePath ();                                // If forward is blocked Change  direction.
  }
  else if ((Distance >= 30) && (Distance < 60)) { // If obstacle  found between 30cm to 60cm.
    digitalWrite (BuzzPIN, LOW);                  //  BuzzPIN output as 0V (Logic low level).
    digitalWrite (LEDBPIN, HIGH);                 //  LEDBPIN output as 5V (Logic high level).
    digitalWrite (LEDGPIN, LOW);                  //  LEDGPIN output as 0V (Logic low level).
    digitalWrite (LEDRPIN, LOW);                  //  LEDRPIN output as 0V (Logic low level).
    M1.setSpeed (150);                            //  Speed increase slightly.
    M2.setSpeed (150);                            //  Speed increase slightly.
    Forward ();                                   //  Robot move to Forward direction.
  }
  else if ((Distance >= 60) && (Distance  < 90)) { // If obstacle found between 60cm to 90cm.
    digitalWrite (BuzzPIN,  LOW);                  // BuzzPIN output as 0V (Logic low level).
    digitalWrite  (LEDBPIN, LOW);                  // LEDBPIN output as 0V (Logic low level).
    digitalWrite (LEDGPIN, HIGH);                 // LEDGPIN output as 5V (Logic  high level).
    digitalWrite (LEDRPIN, LOW);                  // LEDRPIN output  as 0V (Logic low level).
    M1.setSpeed (200);                            //  Speed up.
    M2.setSpeed (200);                            // Speed up.
    Forward ();                                   // Robot move to Forward direction.
  }
  else {                                          // If obstacle cannot be  found in 90cm.
    digitalWrite (BuzzPIN, LOW);                  // BuzzPIN output  as 0V (Logic low level).
    digitalWrite (LEDBPIN, HIGH);                 //  LEDBPIN output as 5V (Logic high level).
    digitalWrite (LEDGPIN, HIGH);                 //  LEDGPIN output as 5V (Logic high level).
    digitalWrite (LEDRPIN, HIGH);                 //  LEDRPIN output as 5V (Logic high level).
    M1.setSpeed (250);                            //  Speed increase fully.
    M2.setSpeed (250);                            // Speed  increase fully.
    Forward ();                                   // Robot move  to Forward direction.
  }
}
void ChangePath () {         // Path Change  loop.
  Stop ();                   // Robot Stop.
  Backward ();               //  Robot run Backward direction.
  Stop ();                   // Robot Stop.
  SER1.write (12);           // Check Distance to the Right.
  delay (500);               //  Delay for 0.5s.
  RightDistance = Search (); // Set Right Distance.
  delay  (500);               // Delay for 0.5s.
  SER1.write (160);          // Check  Distance to the Left.
  delay (1000);              // Delay for 1s.
  LeftDistance  = Search ();  // Set Left Distance.
  delay (500);               // Delay for  0.5s.
  SER1.write (80);           // Return to center.
  delay (500);               //  Delay for 0.5s.
  CompareDistance ();        // Find the longest distance.
}
void  CompareDistance () {                   // Distance Compare loop.
  if (RightDistance  > LeftDistance) {       // If Right is less obstructed.
     TurnRight ();                          //  Robot Turn into Right direction.
    }
  else if (LeftDistance > RightDistance)  {  // If Left is less obstructed.
      TurnLeft ();                          //  Robot Turn into Left direction.
    }
  else {                                    //  If both are equally obstructed.
      TurnAround ();                        //  Robot Turn Around.
    }
}
void Forward () {   // Forward loop.
  M1.run  (FORWARD); // Turn DCMotor #1 to Forward.
  M2.run (FORWARD); // Turn DCMotor  #1 to Forward.
}
void Backward () {    // Backward loop.
  M1.run (BACKWARD);  // Turn DCMotor #1 to Backward.
  M2.run (BACKWARD);  // Turn DCMotor #2 to  Backward.
  delay (500);       // Delay for 1s.
}
void TurnRight () {           //  Right Turn loop.
  M1.run (BACKWARD);          // Turn DCMotor #1 to Backward.
  M2.run (FORWARD);           // Turn DCMotor #2 to Forward.
  M1.setSpeed (100+DCMROFF);  // Calibrate the Speed of DCMotor #1.
  delay (300);                // Delay  for 0.7s.
}
void TurnLeft () {            // Left Turn loop.
  M1.run (FORWARD);           // Turn DCMotor #1 to Forward.
  M2.run (BACKWARD);          // Turn  DCMotor #2 to Backward.
  M2.setSpeed (100+DCMROFF);  // Calibrate the Speed  of DCMotor #2.
  delay (300);                // Delay for 0.7s.
}
void  TurnAround () {          // Trun Around loop.
  M1.run (FORWARD);           //  Turn DCMotor #1 to Forward.
  M2.run (BACKWARD);          // Turn DCMotor #2  to Backward.
  M2.setSpeed (100+DCMROFF);  // Calibrate the Speed of DCMotor  #2.
  delay (700);               // Delay for 2.1s.
}
void Stop () {      //  Stop loop.
  M1.run (RELEASE); // Release DCMotor #1.
  M2.run (RELEASE);  // Release DCMotor #2.
  delay (100);      // Delay for 0.1s.
}

Downloadable files

Diagram

The circuit diagram, for setting up accordingly this autonomous machine.

Diagram

The circuit diagram, for setting up accordingly this autonomous machine.

Diagram

Documentation

Chassis

The robot chassis, for setting up accordingly this autonomous machine.

https://sketchfab.com/models/dd3487cd9fb94dc9af33dd0b5a529b14

Comments

Only logged in users can leave comments

ertezatawsif

0 Followers

•

0 Projects

Intro

138