-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdistance.py
144 lines (120 loc) · 3.53 KB
/
distance.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
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
# (c) stonatm@gmail.com
# Hc-sr04 ultrasonic distance sensor library
class hcsr04:
from machine import Pin
import time
ECHO = None
TRIG = None
def init(echo_pin, trig_pin):
try:
hcsr04.ECHO = hcsr04.Pin(echo_pin, hcsr04.Pin.IN)
hcsr04.TRIG = hcsr04.Pin(trig_pin, hcsr04.Pin.OUT)
except:
pass
def _measure():
if (not hcsr04.ECHO) or not(hcsr04.TRIG):
return 0
#send trigger signal
hcsr04.TRIG.value(0)
hcsr04.time.sleep_us(5)
hcsr04.TRIG.value(1)
hcsr04.time.sleep_us(10)
hcsr04.TRIG.value(0)
#calculate duration of echo signal in microseconds
while hcsr04.ECHO.value() == 0:
pulse_start = hcsr04.time.ticks_us()
while hcsr04.ECHO.value() == 1:
pulse_end = hcsr04.time.ticks_us()
pulse_dur = pulse_end - pulse_start
# calculate distance in cm
distance = pulse_dur * 17150 / 1000000
return int(distance)
def distance_cm():
return hcsr04._measure()
def distance_m():
return hcsr04._measure() / 100
# Sharp GP2Y0A21YK infrared distance sensor
# model: "1080" [10cm to 80cm]
class sharp1080:
import math
ADC_RES = None
ADC_PIN = None
ESP8266 = 1
ESP32 = 2
PICO = 3
INIT = False
DEBUG = False
FAST = False
def init(pin, model):
sharp1080.ADC_PIN = pin
if model == sharp1080.ESP8266:
sharp1080.ADC_RES = 10
sharp1080.INIT = sharp1080.ESP8266
elif model == sharp1080.ESP32:
sharp1080.ADC_RES = 12
sharp1080.INIT = sharp1080.ESP32
elif model == sharp1080.PICO:
sharp1080.ADC_RES = 16
sharp1080.INIT = sharp1080.PICO
else:
sharp1080.INIT = False
# read sensor output voltage
def _read_adc():
# exit if adc resolution was not set
if not sharp1080.INIT:
return 0
from machine import ADC, Pin
conversion_factor = 3.3 / ((2**sharp1080.ADC_RES)-1)
# raspberry pico
if sharp1080.INIT == sharp1080.PICO:
adc = ADC(Pin(sharp1080.ADC_PIN))
raw = adc.read_u16()
# esp8266
if sharp1080.INIT == sharp1080.ESP8266:
adc = ADC(0)
raw = adc.read()
# esp32
if sharp1080.INIT == sharp1080.ESP32:
adc = ADC(Pin(sharp1080.ADC_PIN))
adc.atten(ADC.ATTN_11DB)
adc.width(ADC.WIDTH_12BIT)
raw = adc.read()
volt = raw * conversion_factor
if sharp1080.DEBUG: print("adc_read: " + str(volt) + "V")
return volt
# return precalculated values
def _calculate_distance_table(volt):
if volt >= 2.60: return 10
if volt >= 2.10: return 12
if volt >= 1.85: return 14
if volt >= 1.65: return 15
if volt >= 1.50: return 18
if volt >= 1.39: return 20
if volt >= 1.15: return 25
if volt >= 0.98: return 30
if volt >= 0.85: return 35
if volt >= 0.75: return 40
if volt >= 0.67: return 45
if volt >= 0.61: return 50
if volt >= 0.59: return 55
if volt >= 0.55: return 60
if volt >= 0.50: return 65
if volt >= 0.48: return 70
if volt >= 0.45: return 75
if volt >= 0.42: return 80
if volt >= 0: return 0
# returns approximate values calculated using the function
def _calculate_distance_function(volt):
if volt == 0:
return 0
dist = 1.05652 + (27.7189 / sharp1080.math.pow(volt, 1.22042))
return int(dist)
# return distance in m
def distance_m():
return (sharp1080.distance_cm() / 100)
# return distance in cm
def distance_cm():
if sharp1080.FAST:
return (sharp1080._calculate_distance_table(sharp1080._read_adc()))
else:
return (sharp1080._calculate_distance_function(sharp1080._read_adc()))