UART & Sensors

orchestrateur/sensors/ultrasonicRanger.py

@@ -0,0 +1,32 @@
+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