Obstacle Avoiding Car

1#include <Servo.h>
2#include <NewPing.h>
3
4int LeftSideForward  = 3;
5int LeftSideBackward = 4;
6int RightSideForward = 2;
7int RightSideBackward  = 7;
8int SpeedRight = 5;
9int SpeedLeft = 6;
10
11int trigPin = A2;
12int  echoPin = A1;
13long duration, cm, inches;
14int maximumcm = 250;
15
16NewPing  sonar(trigPin, echoPin, maximumcm);
17Servo servo_motor;
18
19
20void setup(){
21
22  pinMode(RightSideForward, OUTPUT);
23  pinMode(LeftSideForward, OUTPUT);
24  pinMode(LeftSideBackward, OUTPUT);
25  pinMode(RightSideBackward, OUTPUT);
26  pinMode(trigPin, OUTPUT);
27  pinMode(echoPin, INPUT);
28  servo_motor.attach(8);  //our servo pin
29  servo_motor.write(115);
30  delay(2000);
31  cm = readPing();
32  delay(100);
33  cm = readPing();
34  delay(100);
35  cm = readPing();
36  delay(100);
37  cm = readPing();
38  delay(100);
39  Serial.begin(9600);
40}
41
42
43
44
45void  loop(){
46  
47  cm = readPing();
48  int cmRight = 0;
49  int cmLeft = 0;
50  delay(50);
51
52  if (cm < 35){
53    Serial.println(cm);
54    Stop();
55    delay(300);
56    Backward();
57    delay(550);
58    Stop();
59    delay(300);
60    cmRight = lookRight();
61    Serial.println(cmRight);
62    delay(300);
63    cmLeft = lookLeft();
64    Serial.println(cmLeft);
65    delay(300);
66    Stop();
67    delay(300);
68 
69
70     if (cmLeft < 20 && cmRight < 20)
71     {
72      Serial.println(cm);
73      Stop();
74      delay(300);
75      Backward();
76      delay(1300);
77      Stop();
78      delay(300);
79      cmRight = lookRight();
80      Serial.println(cmRight);
81      delay(300);
82      cmLeft = lookLeft();
83      Serial.println(cmLeft);
84      delay(300);
85      Stop();
86      delay(300);
87      
88     }
89     else if (cmLeft > cmRight){
90      Right();
91     delay(520);
92     Stop();
93     }
94      else if (cmRight > cmLeft)
95   {
96     Left();
97     delay(510);
98     Stop();
99   }
100  }
101
102    else
103    {
104     Forward();
105    }
106
107cm = readPing;
108}
109
110//FUNCTIONS
111
112int lookRight(){  
113  servo_motor.write(0);
114  delay(500);
115  int cm = readPing();
116  delay(300);
117  servo_motor.write(90);
118  return cm;
119  delay(150);
120}
121
122int lookLeft(){
123  servo_motor.write(180);
124  delay(500);
125  int cm = readPing();
126  delay(300);
127  servo_motor.write(90);
128  return cm;
129  delay(150);
130}
131
132int readPing(){
133  delay(70);
134  int cm = sonar.ping_cm();
135  if (cm==0){
136    cm=250;
137  }
138  return cm;
139  delay(150);
140}
141
142void Stop(){
143  
144  digitalWrite(LeftSideForward,  LOW);
145  digitalWrite(LeftSideBackward, LOW);
146  digitalWrite(RightSideForward,  LOW);
147  digitalWrite(RightSideBackward, LOW);
148}
149
150void Forward()
151{
152    Serial.println("Forward");
153    digitalWrite(LeftSideForward, HIGH);
154    digitalWrite(LeftSideBackward, LOW);
155    digitalWrite(RightSideForward, HIGH);
156    digitalWrite(RightSideBackward, LOW);
157    analogWrite(SpeedRight, 75);
158    analogWrite(SpeedLeft, 75);
159}
160
161void Backward()
162{
163    Serial.println("Backward");
164    digitalWrite(LeftSideForward, LOW);
165    digitalWrite(LeftSideBackward, HIGH);
166    digitalWrite(RightSideForward, LOW);
167    digitalWrite(RightSideBackward,  HIGH);
168    analogWrite(SpeedRight, 75);
169    analogWrite(SpeedLeft, 75);
170}
171
172void  Right()
173{
174    Serial.println("Right");
175    digitalWrite(LeftSideForward,  HIGH);
176    digitalWrite(LeftSideBackward, LOW);
177    digitalWrite(RightSideForward,  LOW);
178    digitalWrite(RightSideBackward, HIGH);
179    analogWrite(SpeedRight,  105);
180    analogWrite(SpeedLeft, 105);
181}
182
183void Left()
184{
185    Serial.println("Left");
186    digitalWrite(LeftSideForward, LOW);
187    digitalWrite(LeftSideBackward, HIGH);
188    digitalWrite(RightSideForward, HIGH);
189    digitalWrite(RightSideBackward,  LOW);
190    analogWrite(SpeedRight, 105);
191    analogWrite(SpeedLeft, 105);
192}