54 lines
1.5 KiB
Python
54 lines
1.5 KiB
Python
import board
|
|
import digitalio
|
|
import time
|
|
|
|
SPEED_OF_SOUND = 343 # m/s
|
|
TRIGGER_DURATION = 0.00001 # 10 µs
|
|
|
|
# Internal cache to track history per echo pin
|
|
sensor_history = {}
|
|
|
|
def measure_distance(trigger_pin, echo_pin, sample_window=5):
|
|
global sensor_history
|
|
|
|
with digitalio.DigitalInOut(trigger_pin) as trigger, digitalio.DigitalInOut(echo_pin) as echo:
|
|
trigger.direction = digitalio.Direction.OUTPUT
|
|
echo.direction = digitalio.Direction.INPUT
|
|
|
|
# Send trigger pulse
|
|
trigger.value = False
|
|
time.sleep(0.0001)
|
|
trigger.value = True
|
|
time.sleep(TRIGGER_DURATION)
|
|
trigger.value = False
|
|
|
|
# Timeout setup
|
|
start_time = time.monotonic_ns()
|
|
timeout = start_time + 3_000_000 # 3 ms
|
|
|
|
while not echo.value:
|
|
if time.monotonic_ns() > timeout:
|
|
break
|
|
pulse_start = time.monotonic_ns()
|
|
|
|
while echo.value:
|
|
if time.monotonic_ns() > timeout:
|
|
return None
|
|
pulse_end = time.monotonic_ns()
|
|
|
|
# Calculate raw distance
|
|
pulse_duration = (pulse_end - pulse_start) / 1_000_000_000
|
|
distance_cm = (pulse_duration * SPEED_OF_SOUND * 100) / 2
|
|
|
|
# Store reading in history buffer
|
|
key = str(echo_pin) # Use pin identity as key
|
|
if key not in sensor_history:
|
|
sensor_history[key] = []
|
|
history = sensor_history[key]
|
|
history.append(distance_cm)
|
|
if len(history) > sample_window:
|
|
history.pop(0)
|
|
|
|
smoothed = sum(history) / len(history)
|
|
return smoothed
|