Support for Ping (pulseIn Firmata) and other improvements

README.rst

@@ -50,6 +50,15 @@ Otherwise the board will keep sending data to your serial, until it overflows::
     >>> board.analog[0].read()
     0.661440304938
 
+A better option is to use a ``with`` block. This way the Iterator is managed 
+automagically, and you don't have to take care of calling ``board.exit()``
+to stop the Iterator thread when you're done::
+
+    >>> with board:
+    >>>     board.analog[0].enable_reporting()
+    >>>     board.analog[0].read()
+    0.661440304938
+
 If you use a pin more often, it can be worth it to use the ``get_pin`` method
 of the board. It let's you specify what pin you need by a string, composed of
 'a' or 'd' (depending on wether you need an analog or digital pin), the pin
@@ -63,6 +72,7 @@ digital pin 3 as pwm.::
     >>> pin3 = board.get_pin('d:3:p')
     >>> pin3.write(0.6)
 
+
 Board layout
 ============
 
@@ -80,6 +90,37 @@ for the Mega for example::
     ...         'disabled' : (0, 1, 14, 15) # Rx, Tx, Crystal
     ...         }
 
+Ping (pulseIn) support
+======================
+
+If you want to use ultrasonic ranging sensors that use a pulse to
+measure distances (like the very cheap and common ``HC-SR04``
+- see http://www.micropik.com/PDF/HCSR04.pdf),
+you'll need to use a``pulseIn`` compatible Firmata in your board.
+
+You can download it from the ``pulseIn`` branch
+of the Firmata repository:
+https://github.com/jgautier/arduino-1/tree/pulseIn
+
+Just plug the sensor ``Trig`` and ``Echo`` pins
+to a digital pin on your board.
+
+.. image:: examples/ping.png
+
+And then use the ping method on the pin:
+
+    >>> echo_pin = board.get_pin('d:7:o')
+    >>> echo_pin.ping()
+    1204
+
+You can use the ``ping_time_to_distance`` function to convert
+the ping result (echo time) into distance:
+
+    >>> from pyfirmata.util import ping_time_to_distance
+    >>> echo_pin = board.get_pin('d:7:o')
+    >>> ping_time_to_distance(echo_pin.ping())
+    20.485458055607776
+
 Todo
 ====
 

examples/ping.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+#-*- coding: utf-8 -*-
+"""
+Example of usage of the "ping" method of Ping to calculate distances.
+
+For this example to work plug an HC-SR04 ultrasonic ranging sensor
+into the pin 8 (trigger and echo pins on the sensor conected to pin 8)
+as shown in the :file:`ping.png` diagram
+(note: image from https://github.com/rwaldron/johnny-five/wiki/proximity
+ Copyright (c) 2016 The Johnny-Five Contributors).
+"""
+__author__      = "Borja López Soilán <[email protected]>"
+
+import time
+import sys
+sys.path.insert(0, '../') # We want to import the local pyfirmata
+from pyfirmata import Arduino, util
+
+board = util.get_the_board(identifier='ttyACM', timeout=10)
+with board:
+	echo_pin = board.get_pin('d:8:o')
+	while True: 
+
+ 		# Send a ping and get the echo time:
+	    duration = echo_pin.ping()
+
+	    if duration:
+	    	# Normal distance (speed of sound based)
+	    	distance = util.ping_time_to_distance(duration)
+
+	    	# Distance based on calibration points.
+	    	calibration = [(680.0, 10.0), (1460.0, 20.0), (2210.0, 30.0)]
+	    	cal_distance = util.ping_time_to_distance(duration, calibration)
+
+	    	print "Distance: \t%scm \t%scm (calibrated) \t(%ss)" \
+	    				% (distance, cal_distance, duration)
+	    else:
+	    	print "No distance!"
+
+	    time.sleep(0.2)

examples/with_board.py

@@ -0,0 +1,19 @@
+#!/usr/bin/env python
+#-*- coding: utf-8 -*-
+"""
+Example of usage of the "with" statement with the board to read a pin.
+"""
+__author__      = "Borja López Soilán <[email protected]>"
+
+import time
+import sys
+sys.path.insert(0, '../') # We want to import the local pyfirmata
+from pyfirmata import Arduino, util
+
+board = util.get_the_board(identifier='ttyACM', timeout=10)
+with board:
+	input_pin = board.get_pin('d:8:i')
+	while True:
+		value = input_pin.read()
+		print "input_pin = %s" % value
+		time.sleep(1)

pyfirmata/pyfirmata.py

@@ -5,7 +5,7 @@
 
 import serial
 
-from .util import to_two_bytes, two_byte_iter_to_str, pin_list_to_board_dict
+from .util import from_two_bytes, to_two_bytes, two_byte_iter_to_str, pin_list_to_board_dict, Iterator
 
 # Message command bytes (0x80(128) to 0xFF(255)) - straight from Firmata.h
 DIGITAL_MESSAGE = 0x90      # send data for a digital pin
@@ -36,6 +36,7 @@
 
 SERVO_CONFIG = 0x70         # set max angle, minPulse, maxPulse, freq
 STRING_DATA = 0x71          # a string message with 14-bits per char
+PULSE_IN = 0x74             # send a pulse in command (pulseIn Firmata required)
 SHIFT_DATA = 0x75           # a bitstream to/from a shift register
 I2C_REQUEST = 0x76          # send an I2C read/write request
 I2C_REPLY = 0x77            # a reply to an I2C read request
@@ -84,6 +85,7 @@ class Board(object):
     _command = None
     _stored_data = []
     _parsing_sysex = False
+    _iterator = None
 
     def __init__(self, port, layout=None, baudrate=57600, name=None, timeout=None):
         self.sp = serial.Serial(port, baudrate, timeout=timeout)
@@ -116,9 +118,34 @@ def __del__(self):
         The connection with the a board can get messed up when a script is
         closed without calling board.exit() (which closes the serial
         connection). Therefore also do it here and hope it helps.
+        (It seems that it doesn't always work, it's better to use
+            the with statement to be sure that exit gets called properly:
+
+            board = Arduino('/dev/...')
+            with board: # an Iterator gets started automagically.
+                # Do something with the board...
+            # board.exit() get's called automagically.
         """
         self.exit()
 
+    def __enter__(self):
+        """
+        Allows you to use the with statement with the board.
+        It will take care of starting an iterator for you.
+        """
+        self._iterator = Iterator(self)
+        self._iterator.start()
+        return self
+
+    def __exit__(self, exc_type, exc_value, traceback):
+        """
+        Allows you to use the with statement with the board.
+        This will make sure exit() gets called so the connection with
+        the board doesn't get messed.
+        """
+        self.exit()
+        self._iterator = None
+
     def send_as_two_bytes(self, val):
         self.sp.write(bytearray([val % 128, val >> 7]))
 
@@ -162,6 +189,7 @@ def _set_default_handlers(self):
         self.add_cmd_handler(DIGITAL_MESSAGE, self._handle_digital_message)
         self.add_cmd_handler(REPORT_VERSION, self._handle_report_version)
         self.add_cmd_handler(REPORT_FIRMWARE, self._handle_report_firmware)
+        self.add_cmd_handler(PULSE_IN, self._handle_pulse_in)
 
     def auto_setup(self):
         """
@@ -274,8 +302,16 @@ def iterate(self):
             except KeyError:
                 return
             received_data.append(data & 0x0F)
-            while len(received_data) < handler.bytes_needed:
-                received_data.append(ord(self.sp.read()))
+            try:
+                while len(received_data) < handler.bytes_needed:
+                    received_data.append(ord(self.sp.read()))
+            except TypeError:
+                # We'll probably get a
+                # "TypeError: ord() expected a character, but string of length 0 found"
+                # if any self.sp.read() fails due to a timeout.
+                raise IOError("Failed to read 'channel' data from port, " \
+                                + "read %s bytes expected %s bytes" %
+                                    (len(received_data), handler.bytes_needed))
         elif data == START_SYSEX:
             data = ord(self.sp.read())
             handler = self._command_handlers.get(data)
@@ -290,8 +326,17 @@ def iterate(self):
                 handler = self._command_handlers[data]
             except KeyError:
                 return
-            while len(received_data) < handler.bytes_needed:
-                received_data.append(ord(self.sp.read()))
+            try:
+                while len(received_data) < handler.bytes_needed:
+                    received_data.append(ord(self.sp.read()))
+            except TypeError:
+                # We'll probably get a
+                # "TypeError: ord() expected a character, but string of length 0 found"
+                # if any self.sp.read() fails due to a timeout.
+                raise IOError("Failed to read data from port, " \
+                                + "read %s bytes expected %s bytes" %
+                                    (len(received_data), handler.bytes_needed))
+
         # Handle the data
         try:
             handler(*received_data)
@@ -379,6 +424,20 @@ def _handle_report_capability_response(self, *data):
 
         self._layout = pin_list_to_board_dict(pin_spec_list)
 
+    #
+    # Handle PULSE_IN sysex responses.
+    # Requires pulseIn compatible Firmata in the arduino board
+    # (see https://github.com/jgautier/arduino-1/tree/pulseIn).
+    # Used with HC-SR04 ultrasonic ranging sensors
+    # (see http://www.micropik.com/PDF/HCSR04.pdf).
+    #
+    def _handle_pulse_in(self, pin_nr, *data):
+        duration = (from_two_bytes(data[1:3]) << 24) \
+                    + (from_two_bytes(data[3:5]) << 16) \
+                    + (from_two_bytes(data[5:7]) << 8) \
+                    + from_two_bytes(data[7:9])
+        self.digital[pin_nr].last_pulse_in_duration = duration
+        # We can calculate the distance using an HC-SR04 as 'duration / 58.2'
 
 class Port(object):
     """An 8-bit port on the board."""
@@ -445,6 +504,7 @@ def __init__(self, board, pin_number, type=ANALOG, port=None):
         self._mode = (type == DIGITAL and OUTPUT or INPUT)
         self.reporting = False
         self.value = None
+        self.last_pulse_in_duration = None
 
     def __str__(self):
         type = {ANALOG: 'Analog', DIGITAL: 'Digital'}[self.type]
@@ -542,3 +602,73 @@ def write(self, value):
                 value = int(value)
                 msg = bytearray([ANALOG_MESSAGE + self.pin_number, value % 128, value >> 7])
                 self.board.sp.write(msg)
+
+    def ping(self, trigger_mode=1, trigger_duration=10, echo_timeout=65000):
+        """
+        Trigger the pin and wait for a pulseIn echo.
+
+        Used with HC-SR04 ultrasonic ranging sensors
+        (see http://www.micropik.com/PDF/HCSR04.pdf).
+
+        Note: Requires pulseIn compatible Firmata in the arduino board
+              (see https://github.com/jgautier/arduino-1/tree/pulseIn).
+
+        :arg trigger_mode: Uses value as a boolean,
+                        0 to trigger LOW,
+                        1 to trigger HIGH (default, for HC-SR04 modules).
+        :arg trigger_duration: Duration (us) for the trigger signal.
+        :arg echo_timeout: Time (us) to wait for the echo (pulseIn timeout).
+        """
+        if self.mode is not OUTPUT:
+            raise IOError("{0} should be in OUTPUT mode".format(self))
+        if trigger_mode not in (0, 1):
+            raise IOError("trigger_mode should be 0 or 1")
+
+        # This is the protocol to ask for a pulseIn:
+        #       START_SYSEX(0xF0)           // send_sysex(...)
+        #       puseIn/pulseOut(0x74)       // send_sysex(PULSE_IN, ...)
+        #       pin(0-127)
+        #       value(1 or 0, HIGH or LOW)
+        #       pulseOutDuration 0 (LSB)
+        #       pulseOutDuration 0 (MSB)
+        #       pulseOutDuration 1 (LSB)
+        #       pulseOutDuration 1 (MSB)
+        #       pulseOutDuration 2 (LSB)
+        #       pulseOutDuration 2 (MSB)
+        #       pulseOutDuration 3 (LSB)
+        #       pulseOutDuration 3 (MSB)
+        #       pulseInTimeout 0 (LSB)
+        #       pulseInTimeout 0 (MSB)
+        #       pulseInTimeout 1 (LSB)
+        #       pulseInTimeout 1 (MSB)
+        #       pulseInTimeout 2 (LSB)
+        #       pulseInTimeout 2 (MSB)
+        #       pulseInTimeout 3 (LSB)
+        #       pulseInTimeout 3 (MSB)
+        #       END_SYSEX(0xF7)             // send_sysex(...)
+
+        data = bytearray()
+        data.append(self.pin_number)    # Pin number
+        data.append(trigger_mode)       # Trigger mode (1 or 0, HIGH or LOW)
+        trigger_duration_arr = to_two_bytes((trigger_duration >> 24) & 0xFF) \
+                             + to_two_bytes((trigger_duration >> 16) & 0xFF) \
+                             + to_two_bytes((trigger_duration >> 8) & 0xFF) \
+                             + to_two_bytes(trigger_duration & 0xFF)
+        data.extend(trigger_duration_arr)   # pulseOutDuration
+        echo_timeout_arr = to_two_bytes((echo_timeout >> 24) & 0xFF) \
+                             + to_two_bytes((echo_timeout >> 16) & 0xFF) \
+                             + to_two_bytes((echo_timeout >> 8) & 0xFF) \
+                             + to_two_bytes(echo_timeout & 0xFF)
+        data.extend(echo_timeout_arr)       # pulseInTimeout
+
+        self.last_pulse_in_duration = None
+        self.board.send_sysex(PULSE_IN, data)
+
+        # Wait for the reply...
+        waited_for = 0
+        while self.last_pulse_in_duration is None \
+                    and waited_for < (echo_timeout + trigger_duration) / 1000.0 ** 2:
+            time.sleep(0.01) # Sleep for 10 milliseconds
+            waited_for = waited_for + 0.01
+
+        return self.last_pulse_in_duration