Obstacle Avoiding Robot

1//ARDUINO OBSTACLE AVOIDING CAR//
2// Before uploading the code you  have to install the necessary library//
3//AFMotor Library https://learn.adafruit.com/adafruit-motor-shield/library-install  //
4//NewPing Library https://github.com/livetronic/Arduino-NewPing// 
5//Servo  Library https://github.com/arduino-libraries/Servo.git //
6// To Install the libraries  go to sketch >> Include Library >> Add .ZIP File >> Select the Downloaded ZIP files  From the Above links //
7
8
9#include <AFMotor.h>  
10#include <NewPing.h>
11#include  <Servo.h> 
12
13#define TRIG_PIN A0 
14#define ECHO_PIN A1 
15#define MAX_DISTANCE  200 
16#define MAX_SPEED 190 // sets speed of DC  motors
17#define MAX_SPEED_OFFSET  20
18
19NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 
20
21AF_DCMotor motor1(1,  MOTOR12_1KHZ); 
22AF_DCMotor motor2(2, MOTOR12_1KHZ);
23AF_DCMotor motor3(3, MOTOR34_1KHZ);
24AF_DCMotor  motor4(4, MOTOR34_1KHZ);
25Servo myservo;   
26
27boolean goesForward=false;
28int  distance = 100;
29int speedSet = 0;
30
31void setup() {
32
33  myservo.attach(10);  
34  myservo.write(115); 
35  delay(2000);
36  distance = readPing();
37  delay(100);
38  distance = readPing();
39  delay(100);
40  distance = readPing();
41  delay(100);
42  distance = readPing();
43  delay(100);
44}
45
46void loop() {
47 int distanceR  = 0;
48 int distanceL =  0;
49 delay(40);
50 
51 if(distance<=15)
52 {
53  moveStop();
54  delay(100);
55  moveBackward();
56  delay(300);
57  moveStop();
58  delay(200);
59  distanceR = lookRight();
60  delay(200);
61  distanceL = lookLeft();
62  delay(200);
63
64  if(distanceR>=distanceL)
65  {
66    turnRight();
67    moveStop();
68  }else
69  {
70    turnLeft();
71    moveStop();
72  }
73 }else
74 {
75  moveForward();
76 }
77 distance = readPing();
78}
79
80int lookRight()
81{
82    myservo.write(50); 
83    delay(500);
84    int distance = readPing();
85    delay(100);
86    myservo.write(115); 
87    return distance;
88}
89
90int  lookLeft()
91{
92    myservo.write(170); 
93    delay(500);
94    int distance  = readPing();
95    delay(100);
96    myservo.write(115); 
97    return distance;
98    delay(100);
99}
100
101int readPing() { 
102  delay(70);
103  int cm = sonar.ping_cm();
104  if(cm==0)
105  {
106    cm = 250;
107  }
108  return cm;
109}
110
111void moveStop()  {
112  motor1.run(RELEASE); 
113  motor2.run(RELEASE);
114  motor3.run(RELEASE);
115  motor4.run(RELEASE);
116  } 
117  
118void moveForward() {
119
120 if(!goesForward)
121  {
122    goesForward=true;
123    motor1.run(FORWARD);      
124    motor2.run(FORWARD);
125    motor3.run(FORWARD); 
126    motor4.run(FORWARD);     
127   for (speedSet =  0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading  down the batteries too quickly
128   {
129    motor1.setSpeed(speedSet);
130    motor2.setSpeed(speedSet);
131    motor3.setSpeed(speedSet);
132    motor4.setSpeed(speedSet);
133    delay(5);
134   }
135  }
136}
137
138void moveBackward() {
139    goesForward=false;
140    motor1.run(BACKWARD);      
141    motor2.run(BACKWARD);
142    motor3.run(BACKWARD);
143    motor4.run(BACKWARD);  
144  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the  speed up to avoid loading down the batteries too quickly
145  {
146    motor1.setSpeed(speedSet);
147    motor2.setSpeed(speedSet);
148    motor3.setSpeed(speedSet);
149    motor4.setSpeed(speedSet);
150    delay(5);
151  }
152}  
153
154void turnRight() {
155  motor1.run(FORWARD);
156  motor2.run(FORWARD);
157  motor3.run(BACKWARD);
158  motor4.run(BACKWARD);     
159  delay(500);
160  motor1.run(FORWARD);      
161  motor2.run(FORWARD);
162  motor3.run(FORWARD);
163  motor4.run(FORWARD);      
164} 
165 
166void turnLeft() {
167  motor1.run(BACKWARD);     
168  motor2.run(BACKWARD);  
169  motor3.run(FORWARD);
170  motor4.run(FORWARD);   
171  delay(500);
172  motor1.run(FORWARD);     
173  motor2.run(FORWARD);
174  motor3.run(FORWARD);
175  motor4.run(FORWARD);
176}  
177