From 83fb6ae8b4f69d909d0f47a97ddc9b00b8bb668f Mon Sep 17 00:00:00 2001 From: Dave Jones Date: Tue, 9 Feb 2016 14:08:27 +0000 Subject: [PATCH] Fix #114 - ultrasonic sensors Implements support for the HC-SR04 ultrasonic sensor as an input device class named DistanceSensor --- docs/api_exc.rst | 2 + docs/api_input.rst | 12 ++- docs/recipes.rst | 43 +++++++++ gpiozero/__init__.py | 2 + gpiozero/compat.py | 26 ++++++ gpiozero/devices.py | 15 ++- gpiozero/exc.py | 3 + gpiozero/input_devices.py | 188 ++++++++++++++++++++++++++++++++++++++ 8 files changed, 287 insertions(+), 4 deletions(-) diff --git a/docs/api_exc.rst b/docs/api_exc.rst index b0754ae..8e71f3d 100644 --- 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 index 27ad046..b451c3e 100644 --- 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 index e1f0a12..92153a7 100644 --- 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 index b8de050..522afc6 100644 --- a/gpiozero/__init__.py +++ b/gpiozero/__init__.py @@ -16,6 +16,7 @@ from .exc import ( GPIOPinInUse, GPIOPinMissing, GPIOBadQueueLen, + GPIOBadSampleWait, InputDeviceError, OutputDeviceError, OutputDeviceBadValue, @@ -48,6 +49,7 @@ from .input_devices import ( LineSensor, MotionSensor, LightSensor, + DistanceSensor, AnalogInputDevice, MCP3008, MCP3004, diff --git a/gpiozero/compat.py b/gpiozero/compat.py index c09d3db..b21d0d3 100644 --- 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 index 78813ce..a96b9d5 100644 --- a/gpiozero/devices.py +++ b/gpiozero/devices.py @@ -12,12 +12,17 @@ import weakref 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 @@ class GPIOThread(Thread): 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 index f2de8c1..252034e 100644 --- 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 index 01891cc..7b0389d 100644 --- a/gpiozero/input_devices.py +++ b/gpiozero/input_devices.py @@ -572,6 +572,194 @@ LightSensor.wait_for_light = LightSensor.wait_for_active 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).