How to make a Obstacle Avoiding Car

1#include<AFMotor.h>
2#include<NewPing.h>
3#include<Servo.h>
4
5#define  TRIG_PIN A0
6#define ECHO_PIN A1 
7#define MAX_SPEED 190
8#define MAX_SPEED_OFFSET  20
9#define MAX_DISTANCE 200
10
11NewPing sonar(TRIG_PIN,ECHO_PIN,MAX_DISTANCE);
12
13AF_DCMotor  motor2(2,MOTOR12_1KHZ);
14AF_DCMotor motor3(3,MOTOR34_1KHZ);
15
16Servo myservo;
17
18boolean  goesForward = false;
19int distance = 100;
20int speedSet = 0;
21
22void setup()  {
23 myservo.attach(10);
24 myservo.write(110);
25 delay(2000);
26
27 distance  = readPing();
28 delay(100);
29 distance = readPing();
30 delay(100);
31 distance  = readPing();
32 delay(100);
33 distance = readPing();
34 delay(100);
35}
36
37void  loop() {
38  int distanceR = 0;
39  int distanceL = 0;
40  delay(40);
41
42  if(distance<=20)
43  {
44    moveStop();
45    delay(100);
46    moveBackward();
47    delay(300);
48    moveStop();
49    delay(200);
50    distanceR = lookRight();
51    delay(200);
52    distanceL = lookLeft();
53    delay(200);
54  
55
56  if(distanceR >=distanceL)
57  {
58    turnRight();
59    moveStop();
60  }else{
61    turnLeft();
62    moveStop();
63  }
64  
65  }else
66  {
67    moveForward();
68  }
69  distance =readPing();
70
71}
72
73int lookRight()
74{
75  myservo.write(50);
76  delay(500);
77  int distance = readPing();
78  delay(100);
79  myservo.write(115);
80  return distance;
81
82}
83int lookLeft()
84{
85  myservo.write(170);
86  delay(500);
87  int distancee = readPing();
88  delay(100);
89  myservo.write  (115);
90  delay(100);
91}
92int readPing()
93{
94  delay(70);
95  int cm  = sonar.ping_cm();
96  if(cm == 0 )
97  {
98 cm = 250;
99  }
100  return cm;
101}
102void  moveStop(){
103  motor2.run(RELEASE);
104  motor3.run(RELEASE);
105
106  }
107  void  moveForward()
108  {
109    if(!goesForward){
110      goesForward = true;
111      motor2.run(FORWARD);
112      motor3.run(FORWARD);
113
114      for(speedSet = 0;speedSet < MAX_SPEED;speedSet+=2)
115      {
116        motor2.setSpeed(speedSet);
117        motor3.setSpeed(speedSet);
118        delay(5);
119      }
120    }
121  }
122  void moveBackward(){
123    goesForward  = false;
124    motor2.run(BACKWARD);
125    motor3.run(BACKWARD);
126
127    for(speedSet  = 0;speedSet < MAX_SPEED;speedSet+=2)
128    {
129      motor2.setSpeed(speedSet);
130      motor3.setSpeed(speedSet);
131      delay(5);
132    }
133  }
134  void turnLeft()
135  {
136    motor2.run(BACKWARD);
137    motor3.run(FORWARD);
138    delay(500);
139    motor2.run(FORWARD);
140    motor3.run(FORWARD);
141  }
142  void turnRight()
143  {
144    motor2.run(FORWARD);
145    motor3.run(BACKWARD);
146    delay(500);
147    motor2.run(FORWARD);
148    motor3.run(FORWARD);
149
150  }
151
152
153
154
155
156
157
158
159
160
161
162
163  
164