-
Notifications
You must be signed in to change notification settings - Fork 0
/
cordx
111 lines (93 loc) · 2.53 KB
/
cordx
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
import RPi.GPIO as GPIO #Import GPIO library
import time
#Import time library
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM) # programming the GPIO by BCM pin numbers
TRIG = 17
ECHO = 27
led = 22
m11=16
m12=12
m21=21
m22=20
GPIO.setup(TRIG,GPIO.OUT) # initialize GPIO Pin as outputs
GPIO.setup(ECHO,GPIO.IN) # initialize GPIO Pin as input
GPIO.setup(led,GPIO.OUT)
GPIO.setup(m11,GPIO.OUT)
GPIO.setup(m12,GPIO.OUT)
GPIO.setup(m21,GPIO.OUT)
GPIO.setup(m22,GPIO.OUT)
GPIO.output(led, 1)
time.sleep(5)
def stop():
print ("stop")
GPIO.output(m11, 0)
GPIO.output(m12, 0)
GPIO.output(m21, 0)
GPIO.output(m22, 0)
def forward():
GPIO.output(m11, 1)
GPIO.output(m12, 0)
GPIO.output(m21, 1)
GPIO.output(m22, 0)
print ("Forward")
def back():
GPIO.output(m11, 0)
GPIO.output(m12, 1)
GPIO.output(m21, 0)
GPIO.output(m22, 1)
print ("back")
def left():
GPIO.output(m11, 0)
GPIO.output(m12, 0)
GPIO.output(m21, 1)
GPIO.output(m22, 0)
print ("left")
def right():
GPIO.output(m11, 1)
GPIO.output(m12, 0)
GPIO.output(m21, 0)
GPIO.output(m22, 0)
print ("right")
stop()
count=0
while True:
i=0
avgDistance=0
for i in range(5):
GPIO.output(TRIG, False) #Set TRIG as LOW
time.sleep(0.1) #Delay
GPIO.output(TRIG, True) #Set TRIG as HIGH
time.sleep(0.00001) #Delay of 0.00001 seconds
GPIO.output(TRIG, False) #Set TRIG as LOW
while GPIO.input(ECHO)==0: #Check whether the ECHO is LOW
GPIO.output(led, False)
pulse_start = time.time()
while GPIO.input(ECHO)==1: #Check whether the ECHO is HIGH
GPIO.output(led, False)
pulse_end = time.time()
pulse_duration = pulse_end - pulse_start #time to get back the pulse to sensor
distance = pulse_duration * 17150 #Multiply pulse duration by 17150 (34300/2) to get distance
distance = round(distance,2) #Round to two decimal points
avgDistance=avgDistance+distance
avgDistance=avgDistance/5
print avgDistance
flag=0
if avgDistance < 15: #Check whether the distance is within 15 cm range
count=count+1
stop()
time.sleep(1)
back()
time.sleep(1.5)
if (count%3 ==1) & (flag==0):
right()
flag=1
else:
left()
flag=0
time.sleep(1.5)
stop()
time.sleep(1)
else:
forward()
flag=0