Added Ultrasonic sensor

compLib/Ultrasonic.py

@@ -0,0 +1,107 @@
+import time
+import RPi.GPIO as GPIO
+
+trigger_pin = 27
+echo_pin = 22
+speed_of_sound_mps = 340 # in m/s
+
+rising_found = False
+rising_time = 0
+falling_found = False
+falling_time = 0
+
+class Ultrasonic:
+
+    @staticmethod
+    def __send_trigger():
+        GPIO.setup(trigger_pin, GPIO.OUT)
+        GPIO.output(trigger_pin, False) # Ensure a clean transition
+        time.sleep(0.000005)
+        GPIO.output(trigger_pin, True)  # Trigger now
+        time.sleep(0.000010)
+        GPIO.output(trigger_pin, False)
+
+    @staticmethod
+    def __signal_start_callback(channel):
+        global rising_found
+        global rising_time
+        rising_found = True
+        rising_time = time.process_time_ns()
+
+    @staticmethod
+    def __signal_end_callback(channel):
+        global falling_found
+        global falling_time
+        falling_found = True
+        falling_time = time.process_time_ns()
+
+    @staticmethod
+    def __measure_pulse(timeout = 1000000) -> int:
+        """ Measure the length of a pulse in us. When either the pulse doesn't start in [timeout] us
+            or the pulse is longer than [timeout] us, 0 is returned.
+
+        :param timeout: Timeout in us
+        :return: The length of the pulse or 0 on timeout
+        :rtype: int
+        """
+
+        global rising_found
+        global rising_time
+        global falling_found
+        global falling_time
+
+        # Setup pin
+        GPIO.setup(echo_pin, GPIO.IN)
+
+        # Wait for the rising edge
+        rising_found = False
+        GPIO.add_event_detect(echo_pin, GPIO.RISING, callback = Ultrasonic.__signal_start_callback)
+        start = time.process_time_ns()
+        while (time.process_time_ns() < start + timeout * 1000 and not rising_found):
+            pass
+
+        GPIO.remove_event_detect(echo_pin)
+        if (not rising_found):
+            return 0
+
+        # Wait for the falling edge
+        falling_found = False
+        GPIO.add_event_detect(echo_pin, GPIO.FALLING, callback = Ultrasonic.__signal_end_callback)
+        start = time.process_time_ns()
+        while (time.process_time_ns() < start + timeout * 1000 and not falling_found):
+            pass
+
+        GPIO.remove_event_detect(echo_pin)
+        if (not falling_found):
+            return 0
+
+        # Finally, return the time difference
+        return int((falling_time - rising_time) / 1000)
+    
+    @staticmethod
+    def get_distance() -> float:
+        """ Get the distance from the Ultrasonic sensor in cm. 
+            Returns 'NaN' when the result is invalid.
+
+        :return: The distance in cm or 'NaN'
+        :rtype: float
+        """
+        GPIO.setmode(GPIO.BCM)
+
+        # Trigger the measurement
+        Ultrasonic.__send_trigger()
+
+        # The pulse length in us
+        pulse = Ultrasonic.__measure_pulse()
+
+        if (pulse == 0):
+            print("invalid")
+            return float('NaN')
+
+        # Convert the speed of sound from m/s to cm/us for convenience
+        speed_of_sound_cmpus = speed_of_sound_mps / 10000
+
+        # Convert us time to cm distance
+        distance = (pulse / 2) * speed_of_sound_cmpus
+
+        return distance