Obstacle Avoiding Robot Car

1#include<Servo.h>
2Servo myservo; // create a servo object
3int redled=13;  // declare the pins
4int greenled=12;
5
6
7int servopin=9;
8int trigpin=6;
9int  echopin=5;
10float distance;
11float duration;
12int datapin=7;
13int clockpin=8;
14int  latchpin=3;
15void setup()
16{
17 Serial.begin(9600); 
18  pinMode(redled,OUTPUT);  // declare the pinmode
19  pinMode(greenled,OUTPUT);
20  
21  pinMode(servopin,OUTPUT);
22  pinMode(trigpin,OUTPUT);
23  pinMode(echopin,INPUT);
24  pinMode(datapin,OUTPUT);
25  pinMode(clockpin,OUTPUT);
26  pinMode(latchpin,OUTPUT);
27  myservo.attach(servopin);  // declare servo object is connected to servo pin
28  
29}
30int ping() // create  a function
31{
32   digitalWrite(trigpin,LOW);
33  delayMicroseconds(10);
34  
35  digitalWrite(trigpin,HIGH);
36  delayMicroseconds(20);
37 digitalWrite(trigpin,LOW);
38
39   duration= pulseIn(echopin,HIGH);
40   distance= duration*0.034/2; // distance  is in cm
41   return(distance);
42   
43 
44}
45
46void loop()
47{ 
48  myservo.write(90);  // set servo at 90 degree angle
49    float value;
50    value=ping(); // call  the ping function
51Serial.println(value);
52delay(100);
53    if(value>20)
54    {
55      digitalWrite(redled,LOW);
56      digitalWrite(greenled,HIGH);
57      
58      digitalWrite(latchpin,LOW);
59      shiftOut(datapin,clockpin,LSBFIRST,B01010000);  // move the car in forward direction
60      digitalWrite(latchpin,HIGH);
61    }
62    if(value<20&& value>15)
63    {
64      digitalWrite(redled,HIGH);
65      digitalWrite(greenled,HIGH);
66      
67      digitalWrite(latchpin,LOW);
68      shiftOut(datapin,clockpin,LSBFIRST,B01010000);
69      digitalWrite(latchpin,HIGH);
70    }
71    
72    if(value<15 )
73    {
74       digitalWrite(latchpin,LOW);
75      shiftOut(datapin,clockpin,LSBFIRST,B00000000);  // stop the car
76      digitalWrite(latchpin,HIGH);
77      delay(1000);
78       digitalWrite(latchpin,LOW);
79      shiftOut(datapin,clockpin,LSBFIRST,B00101000);  // first back the robo car
80      digitalWrite(latchpin,HIGH);
81      delay(1000);
82      digitalWrite(latchpin,LOW);
83      shiftOut(datapin,clockpin,LSBFIRST,B00000000);  // then again stop 
84      digitalWrite(latchpin,HIGH);
85      delay(100);
86      
87      go(); // call go function
88      delay(2000);
89      
90    }
91    myservo.write(90);
92}
93void go()
94{
95  int a;
96  int b;
97  myservo.write(0);
98  delay(1000);
99  a=ping();
100  myservo.write(180);
101  delay(1000);
102  b=ping();
103  if(a>b)
104  {
105   digitalWrite(redled,HIGH);
106      digitalWrite(greenled,LOW);
107      
108      digitalWrite(latchpin,LOW);
109      shiftOut(datapin,clockpin,LSBFIRST,B00010000);  // move to the right side
110      digitalWrite(latchpin,HIGH);
111     
112      
113  }
114  else if(a<b)
115  {
116    digitalWrite(redled,HIGH);
117      digitalWrite(greenled,LOW);
118     
119      digitalWrite(latchpin,LOW);
120      shiftOut(datapin,clockpin,LSBFIRST,B01000000);  // move to the left side
121      digitalWrite(latchpin,HIGH);
122      
123  }
124  else if(a=b)
125  {
126    digitalWrite(redled,HIGH);
127      digitalWrite(greenled,LOW);
128      
129      digitalWrite(latchpin,LOW);
130      shiftOut(datapin,clockpin,LSBFIRST,B00101000);  // first back the robo car
131      digitalWrite(latchpin,HIGH);
132      delay(4000);  // wait for 4 seconds
133       digitalWrite(latchpin,LOW);
134      shiftOut(datapin,clockpin,LSBFIRST,B01000000);  // then move to right direction
135      digitalWrite(latchpin,HIGH);
136      delay(2000);
137      
138      
139  }
140}
141