Obstacle Avoiding Car

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