PYTHON (Robotics Project)

Robotics/finchconnection.py

# This module implements connection of a Finch robot via USB. It is used by # finch.py to send and receive commands from the Finch. # The Finch is a robot for computer science education. Its design is the result # of a four year study at Carnegie Mellon's CREATE lab. # http://www.finchrobot.com import atexit import os import ctypes import threading import time import platform import sys VENDOR_ID = 0x2354 DEVICE_ID = 0x1111 HIDAPI_LIBRARY_PATH = os.environ.get('HIDAPI_LIB_PATH', './') PING_FREQUENCY_SECONDS = 2.0 # seconds # Detect which operating system is present and load corresponding library system = platform.system() if system == 'Windows': if sys.maxsize > 2**32: hid_api = ctypes.CDLL(os.path.join(HIDAPI_LIBRARY_PATH, "hidapi64.dll")) else: hid_api = ctypes.CDLL(os.path.join(HIDAPI_LIBRARY_PATH, "hidapi32.dll")) elif system == 'Linux': if sys.maxsize > 2**32: hid_api = ctypes.CDLL(os.path.join(HIDAPI_LIBRARY_PATH, "libhidapi64.so")) else: hid_api = ctypes.CDLL(os.path.join(HIDAPI_LIBRARY_PATH, "libhidapi32.so" )) elif system == 'Darwin': hid_api = ctypes.CDLL(os.path.join(HIDAPI_LIBRARY_PATH, "libhidapi.dylib")) else: hid_api = ctypes.CDLL(os.path.join(HIDAPI_LIBRARY_PATH, "libhidapipi.so")) def _inherit_docstring(cls): def doc_setter(method): parent = getattr(cls, method.__name__) method.__doc__ = parent.__doc__ return method return doc_setter class FinchConnection: """ USB connection to the Finch robot. Uses the HID API to read and write from the robot. """ c_finch_handle = ctypes.c_void_p(None) c_io_buffer = ctypes.c_char_p(None) cmd_id = 0 def is_open(self): """Returns True if connected to the robot.""" return bool(self.c_finch_handle) def open(self): """ Connect to the robot. This method looks for a USB port the Finch is connceted to. """ _before_new_finch_connection(self) if self.is_open(): self.close() try: hid_api.hid_open.restype = ctypes.c_void_p self.c_finch_handle = hid_api.hid_open( ctypes.c_ushort(VENDOR_ID), ctypes.c_ushort(DEVICE_ID), ctypes.c_void_p(None)) self.c_io_buffer = ctypes.create_string_buffer(9) _new_finch_connected(self) self.cmd_id = self.read_cmd_id() except: raise Exception("Failed to connect to the Finch robot.") def close(self): """ Disconnect the robot. """ if self.c_finch_handle: self.send(b'R', [0]) # exit to idle (rest) mode hid_api.hid_close.argtypes = [ctypes.c_void_p] hid_api.hid_close(self.c_finch_handle) self.c_finch_handle = ctypes.c_void_p(None) self.c_io_buffer = ctypes.c_char_p(None) global _open_finches if self in _open_finches: _open_finches.remove(self) def read_cmd_id(self): """ Read the robot's internal command counter. """ #self.send('z', receive = True) self.send(b'z') data = self.receive() return data[0] def send(self, command, payload=()): """Send a command to the robot (internal). command: The command ASCII character payload: a list of up to 6 bytes of additional command info """ if not self.is_open(): raise Exception("Connection to Finch was closed.") # Format the buffer to contain the contents of the payload. for i in range(7): self.c_io_buffer[i] = b'\x00' self.c_io_buffer[1] = command[0] python_version = sys.version_info[0] if payload: for i in range(len(payload)): if python_version >= 3: self.c_io_buffer[i+2] = payload[i] else: self.c_io_buffer[i+2] = bytes(chr(payload[i])) # Make sure command id is incremented if this is a receive case else: self.cmd_id = (self.cmd_id + 1) % 256 # Make sure the command id is incremented so that two calls to obstacle() don't cause the system to hang #self.cmd_id = (self.cmd_id + 1) % 256 #if self.cmd_id == 0: # self.cmd_id = 1 if python_version >= 3: self.c_io_buffer[8] = self.cmd_id else: self.c_io_buffer[8] = bytes(chr(self.cmd_id)) # Write to the Finch bufffer res = 0 while not res: hid_api.hid_write.argtypes = [ctypes.c_void_p, ctypes.c_char_p, ctypes.c_size_t] res = hid_api.hid_write(self.c_finch_handle, self.c_io_buffer, ctypes.c_size_t(9)) def receive(self): """ Read the data from the Finch buffer. """ res = 9 while res > 0: hid_api.hid_read.argtypes = [ctypes.c_void_p, ctypes.c_char_p, ctypes.c_size_t] if system == 'Darwin': res = hid_api.hid_read(self.c_finch_handle, self.c_io_buffer, ctypes.c_size_t(9)) else: res = hid_api.hid_read_timeout(self.c_finch_handle, self.c_io_buffer, ctypes.c_size_t(9), 50) if self.cmd_id == ord(self.c_io_buffer[8]): break return [ord(self.c_io_buffer[i]) for i in range(9)] class ThreadedFinchConnection(FinchConnection): """Threaded implementation of Finch Connection""" lock = None thread = None main_thread = None last_cmd_sent = time.time() @_inherit_docstring(FinchConnection) def open(self): FinchConnection.open(self) if not self.is_open(): return self.lock = threading.Lock() self.thread = threading.Thread(target=self.__class__._pinger, args=(self, )) self.main_thread = threading.current_thread() self.thread.start() @_inherit_docstring(FinchConnection) def send(self, command, payload=(), receive=False): try: if self.lock is not None: self.lock.acquire() #FinchConnection.send(self, command, payload=payload, receive=receive) FinchConnection.send(self, command, payload=payload) self.last_cmd_sent = time.time() finally: if self.lock is not None: self.lock.release() @_inherit_docstring(FinchConnection) def receive(self): try: if self.lock is not None: self.lock.acquire() data = FinchConnection.receive(self) finally: if self.lock is not None: self.lock.release() return data def _pinger(self): """ Sends keep-alive commands every few secconds of inactivity. """ while True: if not self.lock: break if not self.c_finch_handle: break if not self.main_thread.isAlive(): break try: self.lock.acquire() now = time.time() if self.last_cmd_sent: delta = now - self.last_cmd_sent else: delta = PING_FREQUENCY_SECONDS if delta >= PING_FREQUENCY_SECONDS: FinchConnection.send(self, b'z') FinchConnection.receive(self) self.last_cmd_sent = now finally: self.lock.release() time.sleep(0.1) @_inherit_docstring(FinchConnection) def close(self): FinchConnection.close(self) self.thread.join() self.lock = None self.thread = None # Functions that handle the list of open finches _open_finches = [] def _before_new_finch_connection(finch): global _open_finches # close other connections for robot in _open_finches: if robot.is_open(): robot.close() def _new_finch_connected(finch): global _open_finches if finch not in _open_finches: _open_finches.append(finch) def _close_all_finches(): global _open_finches if not _open_finches: return for finch in _open_finches: if finch.is_open(): finch.close() atexit.register(_close_all_finches)