Merge pull request gpiozero#184 from waveform80/ultrasonics

waveform80 · waveform80 · commit 3efd522457e5 · 2016-02-12T19:32:24.000Z
Fix gpiozero#114
diff --git a/docs/api_exc.rst b/docs/api_exc.rst
@@ -48,6 +48,8 @@ so you can still do::
 
 .. autoexception:: GPIOBadQueueLen
 
+.. autoexception:: GPIOBadSampleWait
+
 .. autoexception:: InputDeviceError
 
 .. autoexception:: OutputDeviceError
diff --git a/docs/api_input.rst b/docs/api_input.rst
@@ -20,8 +20,8 @@ Button
     :members: wait_for_press, wait_for_release, pin, is_pressed, pull_up, when_pressed, when_released
 
 
-Motion Sensor (PIR)
-===================
+Motion Sensor (D-SUN PIR)
+=========================
 
 .. autoclass:: MotionSensor(pin, queue_len=1, sample_rate=10, threshold=0.5, partial=False)
     :members: wait_for_motion, wait_for_no_motion, pin, motion_detected, when_motion, when_no_motion
@@ -33,6 +33,14 @@ Light Sensor (LDR)
 .. autoclass:: LightSensor(pin, queue_len=5, charge_time_limit=0.01, threshold=0.1, partial=False)
     :members: wait_for_light, wait_for_dark, pin, light_detected, when_light, when_dark
 
+
+Distance Sensor (HC-SR04)
+=========================
+
+.. autoclass:: DistanceSensor(echo, trigger, queue_len=30, max_distance=1, threshold_distance=0.3, partial=False)
+    :members: wait_for_in_range, wait_for_out_of_range, trigger, echo, when_in_range, when_out_of_range, max_distance, distance, threshold_distance
+
+
 Analog to Digital Converters (ADC)
 ==================================
 
diff --git a/docs/recipes.rst b/docs/recipes.rst
@@ -443,6 +443,36 @@ level::
     pause()
 
 
+Distance sensor
+===============
+
+.. IMAGE TBD
+
+Have a :class:`DistanceSensor` detect the distance to the nearest object::
+
+    from gpiozero import DistanceSensor
+    from time import sleep
+
+    sensor = DistanceSensor(23, 24)
+
+    while True:
+        print('Distance to nearest object is', sensor.distance, 'm')
+        sleep(1)
+
+Run a function when something gets near the sensor::
+
+    from gpiozero import DistanceSensor, LED
+    from signal import pause
+
+    sensor = DistanceSensor(23, 24, max_distance=1, threshold_distance=0.2)
+    led = LED(16)
+
+    sensor.when_in_range = led.on
+    sensor.when_out_of_range = led.off
+
+    pause()
+
+
 Motors
 ======
 
@@ -480,6 +510,19 @@ Make a :class:`Robot` drive around in (roughly) a square::
         robot.right()
         sleep(1)
 
+Make a robot with a distance sensor that runs away when things get within
+20cm of it::
+
+    from gpiozero import Robot, DistanceSensor
+    from signal import pause
+
+    sensor = DistanceSensor(23, 24, max_distance=1, threshold_distance=0.2)
+    robot = Robot(left=(4, 14), right=(17, 18))
+
+    sensor.when_in_range = robot.backward
+    sensor.when_out_of_range = robot.stop
+    pause()
+
 
 Button controlled robot
 =======================
diff --git a/gpiozero/__init__.py b/gpiozero/__init__.py
@@ -16,6 +16,7 @@
     GPIOPinInUse,
     GPIOPinMissing,
     GPIOBadQueueLen,
+    GPIOBadSampleWait,
     InputDeviceError,
     OutputDeviceError,
     OutputDeviceBadValue,
@@ -48,6 +49,7 @@
     LineSensor,
     MotionSensor,
     LightSensor,
+    DistanceSensor,
     AnalogInputDevice,
     MCP3008,
     MCP3004,
diff --git a/gpiozero/compat.py b/gpiozero/compat.py
@@ -1,3 +1,5 @@
+# vim: set fileencoding=utf-8:
+
 from __future__ import (
     unicode_literals,
     absolute_import,
@@ -25,3 +27,27 @@ def isclose(a, b, rel_tol=1e-9, abs_tol=0.0):
         (diff <= abs(rel_tol * a)) or
         (diff <= abs_tol)
         )
+
+
+# Backported from py3.4
+def mean(data):
+    if iter(data) is data:
+        data = list(data)
+    n = len(data)
+    if not n:
+        raise ValueError('cannot calculate mean of empty data')
+    return sum(data) / n
+
+
+# Backported from py3.4
+def median(data):
+    data = sorted(data)
+    n = len(data)
+    if not n:
+        raise ValueError('cannot calculate median of empty data')
+    elif n % 2:
+        return data[n // 2]
+    else:
+        i = n // 2
+        return (data[n - 1] + data[n]) / 2
+
diff --git a/gpiozero/devices.py b/gpiozero/devices.py
@@ -12,12 +12,17 @@
 from threading import Thread, Event, RLock
 from collections import deque
 from types import FunctionType
+try:
+    from statistics import median, mean
+except ImportError:
+    from .compat import median, mean
 
 from .exc import (
     GPIOPinMissing,
     GPIOPinInUse,
     GPIODeviceClosed,
     GPIOBadQueueLen,
+    GPIOBadSampleWait,
     )
 
 # Get a pin implementation to use as the default; we prefer RPi.GPIO's here
@@ -344,23 +349,29 @@ def join(self):
 
 
 class GPIOQueue(GPIOThread):
-    def __init__(self, parent, queue_len=5, sample_wait=0.0, partial=False):
+    def __init__(
+            self, parent, queue_len=5, sample_wait=0.0, partial=False,
+            average=median):
         assert isinstance(parent, GPIODevice)
+        assert callable(average)
         super(GPIOQueue, self).__init__(target=self.fill)
         if queue_len < 1:
             raise GPIOBadQueueLen('queue_len must be at least one')
+        if sample_wait < 0:
+            raise GPIOBadSampleWait('sample_wait must be 0 or greater')
         self.queue = deque(maxlen=queue_len)
         self.partial = partial
         self.sample_wait = sample_wait
         self.full = Event()
         self.parent = weakref.proxy(parent)
+        self.average = average
 
     @property
     def value(self):
         if not self.partial:
             self.full.wait()
         try:
-            return sum(self.queue) / len(self.queue)
+            return self.average(self.queue)
         except ZeroDivisionError:
             # No data == inactive value
             return 0.0
diff --git a/gpiozero/exc.py b/gpiozero/exc.py
@@ -28,6 +28,9 @@ class GPIOPinMissing(GPIODeviceError, ValueError):
 class GPIOBadQueueLen(GPIODeviceError, ValueError):
     "Error raised when non-positive queue length is specified"
 
+class GPIOBadSampleWait(GPIODeviceError, ValueError):
+    "Error raised when a negative sampling wait period is specified"
+
 class InputDeviceError(GPIODeviceError):
     "Base class for errors specific to the InputDevice hierarchy"
 
diff --git a/gpiozero/input_devices.py b/gpiozero/input_devices.py
@@ -572,6 +572,194 @@ def _read(self):
 LightSensor.wait_for_dark = LightSensor.wait_for_inactive
 
 
+class DistanceSensor(SmoothedInputDevice):
+    """
+    Extends :class:`SmoothedInputDevice` and represents an HC-SR04 ultrasonic
+    distance sensor, as found in the `CamJam #3 EduKit`_.
+
+    The distance sensor requires two GPIO pins: one for the *trigger* (marked
+    TRIG on the sensor) and another for the *echo* (marked ECHO on the sensor).
+    However, a voltage divider is required to ensure the 5V from the ECHO pin
+    doesn't damage the Pi. Wire your sensor according to the following
+    instructions:
+
+    1. Connect the GND pin of the sensor to a ground pin on the Pi.
+
+    2. Connect the TRIG pin of the sensor a GPIO pin.
+
+    3. Connect a 330Ω resistor from the ECHO pin of the sensor to a different
+       GPIO pin.
+
+    4. Connect a 470Ω resistor from ground to the ECHO GPIO pin. This forms
+       the required voltage divider.
+
+    5. Finally, connect the VCC pin of the sensor to a 5V pin on the Pi.
+
+    The following code will periodically report the distance measured by the
+    sensor in cm assuming the TRIG pin is connected to GPIO17, and the ECHO
+    pin to GPIO18::
+
+        from gpiozero import DistanceSensor
+        from time import sleep
+
+        sensor = DistanceSensor(18, 17)
+        while True:
+            print('Distance: ', sensor.distance * 100)
+            sleep(1)
+
+    :param int echo:
+        The GPIO pin which the ECHO pin is attached to. See :doc:`notes` for
+        valid pin numbers.
+
+    :param int trigger:
+        The GPIO pin which the TRIG pin is attached to. See :doc:`notes` for
+        valid pin numbers.
+
+    :param int queue_len:
+        The length of the queue used to store values read from the sensor.
+        This defaults to 30.
+
+    :param float max_distance:
+        The :attr:`value` attribute reports a normalized value between 0 (too
+        close to measure) and 1 (maximum distance). This parameter specifies
+        the maximum distance expected in meters. This defaults to 1.
+
+    :param float threshold_distance:
+        Defaults to 0.3. This is the distance (in meters) that will trigger the
+        ``in_range`` and ``out_of_range`` events when crossed.
+
+    :param bool partial:
+        When ``False`` (the default), the object will not return a value for
+        :attr:`~SmoothedInputDevice.is_active` until the internal queue has
+        filled with values.  Only set this to ``True`` if you require values
+        immediately after object construction.
+
+    .. _CamJam #3 EduKit: http://camjam.me/?page_id=1035
+    """
+    def __init__(
+            self, echo=None, trigger=None, queue_len=30, max_distance=1,
+            threshold_distance=0.3, partial=False):
+        if not (max_distance > 0):
+            raise ValueError('invalid maximum distance (must be positive)')
+        self._trigger = None
+        super(DistanceSensor, self).__init__(
+            echo, pull_up=False, threshold=threshold_distance / max_distance,
+            queue_len=queue_len, sample_wait=0.0, partial=partial
+        )
+        try:
+            self.speed_of_sound = 343.26 # m/s
+            self._max_distance = max_distance
+            self._trigger = GPIODevice(trigger)
+            self._echo = Event()
+            self._trigger.pin.function = 'output'
+            self._trigger.pin.state = False
+            self.pin.edges = 'both'
+            self.pin.bounce = None
+            self.pin.when_changed = self._echo.set
+            self._queue.start()
+        except:
+            self.close()
+            raise
+
+    def close(self):
+        try:
+            self._trigger.close()
+        except AttributeError:
+            if self._trigger is not None:
+                raise
+        else:
+            self._trigger = None
+        super(DistanceSensor, self).close()
+
+    @property
+    def max_distance(self):
+        """
+        The maximum distance that the sensor will measure in meters. This value
+        is specified in the constructor and is used to provide the scaling
+        for the :attr:`value` attribute. When :attr:`distance` is equal to
+        :attr:`max_distance`, :attr:`value` will be 1.
+        """
+        return self._max_distance
+
+    @max_distance.setter
+    def max_distance(self, value):
+        if not (value > 0):
+            raise ValueError('invalid maximum distance (must be positive)')
+        t = self.threshold_distance
+        self._max_distance = value
+        self.threshold_distance = t
+
+    @property
+    def threshold_distance(self):
+        """
+        The distance, measured in meters, that will trigger the
+        :attr:`when_in_range` and :attr:`when_out_of_range` events when
+        crossed. This is simply a meter-scaled variant of the usual
+        :attr:`threshold` attribute.
+        """
+        return self.threshold * self.max_distance
+
+    @threshold_distance.setter
+    def threshold_distance(self, value):
+        self.threshold = value / self.max_distance
+
+    @property
+    def distance(self):
+        """
+        Returns the current distance measured by the sensor in meters. Note
+        that this property will have a value between 0 and
+        :attr:`max_distance`.
+        """
+        return self.value * self._max_distance
+
+    @property
+    def trigger(self):
+        """
+        Returns the :class:`Pin` that the sensor's trigger is connected to.
+        """
+        return self._trigger.pin
+
+    @property
+    def echo(self):
+        """
+        Returns the :class:`Pin` that the sensor's echo is connected to. This
+        is simply an alias for the usual :attr:`pin` attribute.
+        """
+        return self.pin
+
+    def _read(self):
+        # Make sure the echo event is clear
+        self._echo.clear()
+        # Fire the trigger
+        self._trigger.pin.state = True
+        sleep(0.00001)
+        self._trigger.pin.state = False
+        # Wait up to 1 second for the echo pin to rise
+        if self._echo.wait(1):
+            start = time()
+            self._echo.clear()
+            # Wait up to 40ms for the echo pin to fall (35ms is maximum pulse
+            # time so any longer means something's gone wrong). Calculate
+            # distance as time for echo multiplied by speed of sound divided by
+            # two to compensate for travel to and from the reflector
+            if self._echo.wait(0.04):
+                distance = (time() - start) * self.speed_of_sound / 2.0
+                return min(1.0, distance / self._max_distance)
+            else:
+                # If we only saw one edge it means we missed the echo because
+                # it was too fast; report minimum distance
+                return 0.0
+        else:
+            # The echo pin never rose or fell; something's gone horribly
+            # wrong (XXX raise a warning?)
+            return 1.0
+
+DistanceSensor.when_out_of_range = DistanceSensor.when_activated
+DistanceSensor.when_in_range = DistanceSensor.when_deactivated
+DistanceSensor.wait_for_out_of_range = DistanceSensor.wait_for_active
+DistanceSensor.wait_for_in_range = DistanceSensor.wait_for_inactive
+
+
 class AnalogInputDevice(CompositeDevice):
     """
     Represents an analog input device connected to SPI (serial interface).
