-
Notifications
You must be signed in to change notification settings - Fork 1
/
sonar.py
103 lines (86 loc) · 3.18 KB
/
sonar.py
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
# Measure distance using an ultrasonic module
# in a loop.
import time
import RPi.GPIO as GPIO
from config import *
# Use BCM GPIO references instead of physical pin numbers
GPIO.setmode(GPIO.BCM)
# Speed of sound in cm/s at temperature
temperature = 20
speedSound = 33100 + (0.6*temperature)
# Set pins as output and input
#GPIO.setup(GPIO_TRIGGER,GPIO.OUT) # Trigger
#GPIO.setup(GPIO_ECHO,GPIO.IN) # Echo
# Set trigger to False (Low)
#GPIO.output(GPIO_TRIGGER, False)
# This function measures a distance
def measure(GPIO_TRIGGER, GPIO_ECHO):
GPIO.output(GPIO_TRIGGER, True) #set trig to high
time.sleep(0.00001)
GPIO.output(GPIO_TRIGGER, False) #After 0.01 ms set trig to low
start = time.time()
stop = time.time()
#save start time
while GPIO.input(GPIO_ECHO) == 0:
start = time.time()
#save stop time
while GPIO.input(GPIO_ECHO) == 1:
stop = time.time()
elapsed = stop-start
distance = (elapsed * 34300)/2
return distance
def measure_average(GPIO_TRIGGER, GPIO_ECHO):
# This function takes 3 measurements and returns the average.
distance1=measure(GPIO_TRIGGER, GPIO_ECHO)
time.sleep(0.1)
distance2=measure(GPIO_TRIGGER, GPIO_ECHO)
time.sleep(0.1)
distance3=measure(GPIO_TRIGGER, GPIO_ECHO)
if SONAR_DEBUG: print("TRIG: {:.0f} ECHO: {:.0f} DIST: {:.2f}".format(GPIO_TRIGGER,GPIO_ECHO,distance1))
#((distance1 + distance2 + distance3) / 3)
return (distance1 + distance2 + distance3) / 3
def measure_better_average(GPIO_TRIGGER, GPIO_ECHO):
# This function takes 3 measurements and returns the average.
distance1=measure(GPIO_TRIGGER, GPIO_ECHO)
time.sleep(0.001)
distance2=measure(GPIO_TRIGGER, GPIO_ECHO)
time.sleep(0.001)
distance3=measure(GPIO_TRIGGER, GPIO_ECHO)
time.sleep(0.001)
distance4=measure(GPIO_TRIGGER, GPIO_ECHO)
time.sleep(0.001)
distance5=measure(GPIO_TRIGGER, GPIO_ECHO)
distance = (distance1 + distance2 + distance3 + distance4 + distance5) / 5
return distance
# This function takes in a distance and returns a boolean.
def within_sonar_range(CM):
print("Ultrasonic Measurement: within_sonar_range")
if measure_average() < CM: return True #Try 1
elif measure_average() < CM: return True #Try 2
elif measure_average() < CM: return True #Try 3
else: return False
if __name__ =="__main__":
#Hello message
print("Ultrasonic Measurement: sonar.py")
print("Speed of sound is",speedSound/100,"m/s at ",temperature,"deg")
# Allow module to settle
# time.sleep(1)
# Wrap main content in a try block so we can
# catch the user pressing CTRL-C and run the
# GPIO cleanup function. This will also prevent
# the user seeing lots of unnecessary error
# messages.
try:
min = 1000
while True:
distance = measure_better_average()
centimeters = distance
print("Distance : {0:5.1f}".format(distance), "cm")
#print "Distance : %.1f" % distance
if centimeters < min:
min = centimeters
time.sleep(1)
except KeyboardInterrupt:
# User pressed CTRL-C
# Reset GPIO settings
GPIO.cleanup()