32 lines
1.1 KiB
Python
32 lines
1.1 KiB
Python
import grovepi
|
|
from sensors.lock import grove_lock
|
|
|
|
# Connect the Grove Ultrasonic Ranger to digital port D4
|
|
# SIG,NC,VCC,GND
|
|
ULTRASONIC_RANGER_PORT = 4
|
|
|
|
def read_ultrasonic_ranger(ultrasonic_ranger=ULTRASONIC_RANGER_PORT):
|
|
if not grove_lock.acquire(timeout=1.0):
|
|
print("Ultrasonic: Lock acquisition timed out")
|
|
return None
|
|
|
|
try:
|
|
return grovepi.ultrasonicRead(ultrasonic_ranger)
|
|
except Exception as e:
|
|
print(f"Error: {e}")
|
|
return None
|
|
finally:
|
|
grove_lock.release()
|
|
|
|
def get_dish_height():
|
|
"""Returns the height of the dish in centimeters."""
|
|
distance = read_ultrasonic_ranger()
|
|
if distance is not None:
|
|
# Assuming the ultrasonic sensor is mounted at a fixed height above the dish
|
|
# and pointing downwards, we can calculate the height of the dish.
|
|
# For example, if the sensor is 30 cm above the dish when it's empty:
|
|
SENSOR_HEIGHT = 30 # cm
|
|
dish_height = SENSOR_HEIGHT - distance
|
|
return max(dish_height, 0) # Ensure height is not negative
|
|
else:
|
|
return None |