From 1bd020d21533f928d0c8b6d48ac04d66f6a1aee4 Mon Sep 17 00:00:00 2001 From: Alan Aguiar Date: Wed, 16 Oct 2013 01:22:25 +0000 Subject: update pybot --- diff --git a/pybot/__init__.py b/pybot/__init__.py index d908414..17acd95 100755..100644 --- a/pybot/__init__.py +++ b/pybot/__init__.py @@ -1,6 +1,7 @@ #! /usr/bin/env python # -*- coding: utf-8 -*- # +# Copyright (c) 2012-2013 Alan Aguiar alanjas@hotmail.com # Copyright (c) 2012-2013 Butiá Team butia@fing.edu.uy # Butia is a free and open robotic platform # www.fing.edu.uy/inco/proyectos/butia @@ -23,5 +24,9 @@ import os import sys # Make sure that can import all files -sys.path.insert(0, os.path.dirname(__file__)) +abs_path = os.path.abspath(os.path.dirname(__file__)) +sys.path.insert(0, abs_path) + +# version = 0.turtlebots_version.secondary_number +__version__ = '0.21.0' diff --git a/pybot/baseboard.py b/pybot/baseboard.py index fe5bba5..7ae28f6 100755..100644 --- a/pybot/baseboard.py +++ b/pybot/baseboard.py @@ -3,6 +3,7 @@ # # Baseboard abstraction for USB4butia # +# Copyright (c) 2012-2013 Alan Aguiar alanjas@hotmail.com # Copyright (c) 2012-2013 Butiá Team butia@fing.edu.uy # Butia is a free and open robotic platform # www.fing.edu.uy/inco/proyectos/butia @@ -23,25 +24,25 @@ NULL_BYTE = 0x00 -DEFAULT_PACKET_SIZE = 0x04 -GET_USER_MODULES_SIZE_COMMAND = 0x05 -GET_USER_MODULE_LINE_COMMAND = 0x06 -GET_HANDLER_SIZE_COMMAND = 0x0A -GET_HANDLER_TYPE_COMMAND = 0x0B -ADMIN_HANDLER_SEND_COMMAND = 0x00 -CLOSEALL_BASE_BOARD_COMMAND = 0x07 -SWITCH_TO_BOOT_BASE_BOARD_COMMAND = 0x09 -RESET_BASE_BOARD_COMMAND = 0xFF - -ADMIN_MODULE_IN_ENDPOINT = 0x01 -ADMIN_MODULE_OUT_ENDPOINT = 0x81 -GET_USER_MODULE_LINE_PACKET_SIZE = 0x05 -GET_LINES_RESPONSE_PACKET_SIZE = 5 -GET_LINE_RESPONSE_PACKET_SIZE = 12 -GET_HANDLER_TYPE_PACKET_SIZE = 5 -GET_HANDLER_RESPONSE_PACKET_SIZE = 5 -CLOSEALL_BASE_BOARD_RESPONSE_PACKET_SIZE = 5 +ADMIN_MODULE_IN_ENDPOINT = 0x01 +ADMIN_MODULE_OUT_ENDPOINT = 0x81 + +DEFAULT_PACKET_SIZE = 0x04 +GET_USER_MODULES_SIZE_COMMAND = 0x05 +GET_USER_MODULE_LINE_COMMAND = 0x06 +GET_HANDLER_SIZE_COMMAND = 0x0A +GET_HANDLER_TYPE_COMMAND = 0x0B +ADMIN_HANDLER_SEND_COMMAND = 0x00 +CLOSEALL_COMMAND = 0x07 +CLOSEALL_RESPONSE_PACKET_SIZE = 0x05 +SWITCH_TO_BOOT_BASE_BOARD_COMMAND = 0x09 +RESET_BASE_BOARD_COMMAND = 0xFF +GET_USER_MODULE_LINE_PACKET_SIZE = 0x05 +GET_LINES_RESPONSE_PACKET_SIZE = 0x05 +GET_LINE_RESPONSE_PACKET_SIZE = 0x0C +GET_HANDLER_TYPE_PACKET_SIZE = 0x05 +GET_HANDLER_RESPONSE_PACKET_SIZE = 0x05 ERROR = -1 @@ -54,6 +55,10 @@ class Baseboard(): self.devices = {} self.openables_loaded = [] + def _debug(self, message, err=''): + if self.debug: + print message, err + def open_baseboard(self): """ Open the baseboard @@ -91,6 +96,13 @@ class Baseboard(): if not(name in self.openables_loaded): self.openables_loaded.append(name) + def remove_openable_loaded(self, name): + """ + Remove the name of device that was opened + """ + if name in self.openables_loaded: + self.openables_loaded.remove(name) + def get_openables_loaded(self): """ Get the list of modules that was openened (no pnp) @@ -109,12 +121,12 @@ class Baseboard(): """ self.listi[number] = name - def get_listi(self): + def get_listi(self, force=False): """ Get the listi: the list of modules present in the board that can be opened (or pnp module opens) """ - if (self.listi == {}): + if (self.listi == {}) or force: self.generate_listi() return self.listi @@ -127,12 +139,10 @@ class Baseboard(): try: s = self.get_user_modules_size() for m in range(s): - name = self.get_user_module_line(m) - self.listi[m] = name + self.listi[m] = self.get_user_module_line(m) except: self.listi = {} - if self.debug: - print 'error listi' + self._debug('ERROR:baseboard listi') def get_device_handler(self, name): """ @@ -156,90 +166,58 @@ class Baseboard(): """ Get the size of the list of user modules (listi) """ - w = [] - w.append(ADMIN_HANDLER_SEND_COMMAND) - w.append(DEFAULT_PACKET_SIZE) - w.append(NULL_BYTE) + w = [ADMIN_HANDLER_SEND_COMMAND, DEFAULT_PACKET_SIZE, NULL_BYTE] w.append(GET_USER_MODULES_SIZE_COMMAND) self.dev.write(w) - raw = self.dev.read(GET_USER_MODULE_LINE_PACKET_SIZE) - - if self.debug: - print 'baseboard:get_user_modules_size return', raw - + self._debug('baseboard:get_user_modules_size', raw) return raw[4] def get_user_module_line(self, index): """ Get the name of device with index: index (listi) """ - w = [] - w.append(ADMIN_HANDLER_SEND_COMMAND) - w.append(GET_USER_MODULE_LINE_PACKET_SIZE) - w.append(NULL_BYTE) + w = [ADMIN_HANDLER_SEND_COMMAND, GET_USER_MODULE_LINE_PACKET_SIZE, NULL_BYTE] w.append(GET_USER_MODULE_LINE_COMMAND) w.append(index) self.dev.write(w) - raw = self.dev.read(GET_LINE_RESPONSE_PACKET_SIZE) - - if self.debug: - print 'baseboard:get_user_module_line return', raw - + self._debug('baseboard:get_user_module_line', raw) c = raw[4:len(raw)] t = '' for e in c: if not(e == NULL_BYTE): t = t + chr(e) - return t def get_handler_size(self): """ Get the number of handlers opened """ - w = [] - w.append(ADMIN_HANDLER_SEND_COMMAND) - w.append(DEFAULT_PACKET_SIZE) - w.append(NULL_BYTE) + w = [ADMIN_HANDLER_SEND_COMMAND, DEFAULT_PACKET_SIZE, NULL_BYTE] w.append(GET_HANDLER_SIZE_COMMAND) self.dev.write(w) - raw = self.dev.read(GET_HANDLER_RESPONSE_PACKET_SIZE) - - if self.debug: - print 'baseboard:get_handler_size return', raw - + self._debug('baseboard:get_handler_size', raw) return raw[4] def get_handler_type(self, index): """ Get the type of the handler: index (return listi index) """ - w = [] - w.append(ADMIN_HANDLER_SEND_COMMAND) - w.append(GET_HANDLER_TYPE_PACKET_SIZE) - w.append(NULL_BYTE) + w = [ADMIN_HANDLER_SEND_COMMAND, GET_HANDLER_TYPE_PACKET_SIZE, NULL_BYTE] w.append(GET_HANDLER_TYPE_COMMAND) w.append(index) self.dev.write(w) - raw = self.dev.read(GET_HANDLER_RESPONSE_PACKET_SIZE) - - if self.debug: - print 'baseboard:get_handler_type return', raw - + self._debug('baseboard:get_handler_type', raw) return raw[4] def switch_to_bootloader(self): """ Admin module command to switch to bootloader """ - w = [] - w.append(ADMIN_HANDLER_SEND_COMMAND) - w.append(DEFAULT_PACKET_SIZE) - w.append(NULL_BYTE) + w = [ADMIN_HANDLER_SEND_COMMAND, DEFAULT_PACKET_SIZE, NULL_BYTE] w.append(SWITCH_TO_BOOT_BASE_BOARD_COMMAND) self.dev.write(w) @@ -247,10 +225,7 @@ class Baseboard(): """ Admin module command to reset the board """ - w = [] - w.append(ADMIN_HANDLER_SEND_COMMAND) - w.append(DEFAULT_PACKET_SIZE) - w.append(NULL_BYTE) + w = [ADMIN_HANDLER_SEND_COMMAND, DEFAULT_PACKET_SIZE, NULL_BYTE] w.append(RESET_BASE_BOARD_COMMAND) self.dev.write(w) @@ -258,18 +233,10 @@ class Baseboard(): """ Admin module command to force close all opened modules """ - w = [] - w.append(ADMIN_HANDLER_SEND_COMMAND) - w.append(DEFAULT_PACKET_SIZE) - w.append(NULL_BYTE) - w.append(CLOSEALL_BASE_BOARD_COMMAND) + w = [ADMIN_HANDLER_SEND_COMMAND, DEFAULT_PACKET_SIZE, NULL_BYTE] + w.append(CLOSEALL_COMMAND) self.dev.write(w) - - raw = self.dev.read(CLOSEALL_BASE_BOARD_RESPONSE_PACKET_SIZE) - - if self.debug: - print 'baseboard:force_close_all return', raw - + raw = self.dev.read(CLOSEALL_RESPONSE_PACKET_SIZE) + self._debug('baseboard:force_close_all', raw) return raw[4] - diff --git a/pybot/com_chotox.py b/pybot/com_chotox.py new file mode 100644 index 0000000..92a34bf --- /dev/null +++ b/pybot/com_chotox.py @@ -0,0 +1,273 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- +# +# Chotox utility for debug +# +# Copyright (c) 2012-2013 Alan Aguiar alanjas@hotmail.com +# Copyright (c) 2012-2013 Butiá Team butia@fing.edu.uy +# Butia is a free and open robotic platform +# www.fing.edu.uy/inco/proyectos/butia +# Facultad de Ingeniería - Universidad de la República - Uruguay +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +import os +import imp +import inspect +from functions import ButiaFunctions +import random + +ERROR = -1 + +class Chotox(ButiaFunctions): + + def __init__(self, debug=False, get_modules=True, chotox=False): + self._debug_flag = debug + self._hotplug = [] + self._openables = [] + self._drivers_loaded = {} + self._get_all_drivers() + self.devices = {0:'admin', 2:'button', 4:'grey', 5:'distanc', 7:'pnp'} + self._openables_loaded = ['admin', 'pnp'] + if get_modules: + self.getModulesList(refresh=False) + + def _debug(self, message, err=''): + if self._debug_flag: + print message, err + + def getButiaCount(self): + """ + Gets the number of boards detected + """ + return 1 + + def getModulesList(self, normal=True, refresh=True): + """ + Get the list of modules loaded in the board + """ + self._debug('=Listing Devices') + modules = [] + self._debug('===board', 0) + for i in range(12): + if self.devices.has_key(i): + module_name = self.devices[i] + elif i < 7: + module_name = 'port' + if self.devices.has_key(i) or (i < 7): + complete_name = module_name + ':' + str(i) + modules.append(complete_name) + self._debug('=====module ' + module_name + (9 - len(module_name)) * ' ' + complete_name) + + return modules + + def _get_all_drivers(self): + """ + Load the drivers for the differents devices + """ + # current folder + path_drivers = os.path.join(os.path.dirname(__file__), 'drivers') + self._debug('Searching drivers in: ', str(path_drivers)) + # normal drivers + tmp = os.listdir(path_drivers) + tmp.sort() + for d in tmp: + if d.endswith('.py'): + name = d.replace('.py', '') + self._openables.append(name) + self._get_driver(path_drivers, name) + # hotplug drivers + path = os.path.join(path_drivers, 'hotplug') + tmp = os.listdir(path) + tmp.sort() + for d in tmp: + if d.endswith('.py'): + name = d.replace('.py', '') + self._hotplug.append(name) + self._get_driver(path, name) + + def _get_driver(self, path, driver): + """ + Get a specify driver + """ + self._debug('Loading driver %s...' % driver) + abs_path = os.path.abspath(os.path.join(path, driver + '.py')) + try: + self._drivers_loaded[driver] = imp.load_source(driver, abs_path) + except: + self._debug('ERROR:usb4butia:_get_driver cannot load %s' % driver, abs_path) + + def callModule(self, modulename, board_number, number, function, params = [], ret_type = int): + """ + Call one function: function for module: modulename in board: board_name + with handler: number (only if the module is pnp, else, the parameter is + None) with parameteres: params + """ + self._open_or_validate(modulename, board_number) + + #print modulename, function + + if modulename == 'butia' and function == 'getVolt': + return 10.5 + elif modulename == 'motors' and function == 'getType': + return 1 + + if function == 'getValue': + if modulename == 'button': + return random.randrange(0, 2) + elif modulename == 'grey' or modulename == 'distanc': + return random.randrange(0, 65536) + else: + return ERROR + elif function == 'getVersion': + if modulename == 'admin': + return 6 + else: + return 1 + else: + return ERROR + + def refresh(self): + """ + Search for connected USB4Butia boards and open it + """ + pass + + def close(self): + """ + Closes all open baseboards + """ + pass + + def moduleOpen(self, mod): + """ + Open the module mod + """ + split = self._split_module(mod) + modulename = split[1] + b = int(split[2]) + if len(self._bb) < (b + 1): + return ERROR + board = self._bb[b] + return self._open_or_validate(modulename, board) + + def _open_or_validate(self, modulename, board): + """ + Open o check if modulename module is open in board: board + """ + if modulename in self._openables: + if modulename in self._openables_loaded: + return self._get_handler(modulename) + else: + m = self._max_handler() + m = m + 1 + self.devices[m] = modulename + self._openables_loaded.append(modulename) + return m + return ERROR + + def moduleClose(self, mod): + """ + Close the module mod + """ + split = self._split_module(mod) + modulename = split[1] + if modulename in self._openables: + b = int(split[2]) + if len(self._bb) < (b + 1): + return ERROR + board = self._bb[b] + if modulename in board.get_openables_loaded(): + number = board.get_device_handler(modulename) + try: + res = board.devices[number].moduleClose() + if res == 1: + board.remove_openable_loaded(modulename) + return res + except Exception, err: + self._debug('ERROR:usb4butia:moduleClose', err) + return ERROR + else: + self._debug('cannot close no opened module') + return ERROR + else: + self._debug('cannot close no openable module') + return ERROR + + def getListi(self, board_number=0): + listi = ['admin', 'pnp', 'port', 'ax', 'button', 'hackp', 'motors', 'butia', 'led'] + listi = listi + ['grey', 'light', 'res', 'volt', 'temp', 'distanc'] + return listi + + def _split_module(self, mbn): + """ + Split a modulename: module@board:port to (number, modulename, board) + """ + board = '0' + number = '0' + if mbn.count('@') > 0: + modulename, bn = mbn.split('@') + if bn.count(':') > 0: + board, number = bn.split(':') + else: + board = bn + else: + if mbn.count(':') > 0: + modulename, number = mbn.split(':') + else: + modulename = mbn + return (number, modulename, board) + + def describe(self, mod): + """ + Describe the functions of a modulename + """ + split = self._split_module(mod) + mod = split[1] + funcs = [] + d = {} + if self._drivers_loaded.has_key(mod): + driver = self._drivers_loaded[mod] + a = dir(driver) + flag = False + for p in a: + if p == '__package__': + flag = True + if flag: + funcs.append(p) + funcs.remove('__package__') + for f in funcs: + h = getattr(driver, f) + i = inspect.getargspec(h) + parameters = i[0] + if 'dev' in parameters: + parameters.remove('dev') + d[f] = parameters + return d + + def _get_handler(self, name): + for e in self.devices: + if self.devices[e] == name: + return e + return ERROR + + def _max_handler(self): + m = ERROR + for e in self.devices: + if e > m: + m = e + return m + + diff --git a/pybot/com_usb.py b/pybot/com_usb.py index 54305ba..ad8891f 100755..100644 --- a/pybot/com_usb.py +++ b/pybot/com_usb.py @@ -3,6 +3,7 @@ # # USB comunication with USB4butia (USB4all) board # +# Copyright (c) 2012-2013 Alan Aguiar alanjas@hotmail.com # Copyright (c) 2012-2013 Butiá Team butia@fing.edu.uy # Butia is a free and open robotic platform # www.fing.edu.uy/inco/proyectos/butia @@ -21,7 +22,6 @@ # along with this program; if not, write to the Free Software # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - import usb USB4ALL_VENDOR = 0x04d8 @@ -41,48 +41,39 @@ ERROR = -1 class usb_device(): def __init__(self, dev): - self.device = dev - self.handle = None - self.debug = True + self.dev = dev + self.debug = False + + def _debug(self, message, err=''): + if self.debug: + print message, err def open_device(self): """ Open the baseboard, configure the interface """ try: - self.handle = self.device.open() - self.handle.setConfiguration(USB4ALL_CONFIGURATION) - self.handle.claimInterface(USB4ALL_INTERFACE) + if self.dev.is_kernel_driver_active(USB4ALL_INTERFACE): + self.dev.detach_kernel_driver(USB4ALL_INTERFACE) + self.dev.set_configuration(USB4ALL_CONFIGURATION) except usb.USBError, err: - if self.debug: - print err - self.handle = None + self._debug('ERROR:com_usb:open_device', err) raise - return self.handle def close_device(self): """ Close the comunication with the baseboard """ - try: - if self.handle: - self.handle.releaseInterface() - except Exception, err: - if self.debug: - print err - raise - self.handle = None - self.device = None + self.dev = None - def read(self, length): + def read(self, size): """ Read from the device length bytes """ try: - return self.handle.bulkRead(ADMIN_MODULE_OUT_ENDPOINT, length, TIMEOUT) - except: - if self.debug: - print 'Exception in read usb' + return self.dev.read(ADMIN_MODULE_OUT_ENDPOINT, size, USB4ALL_INTERFACE, TIMEOUT) + except Exception, err: + self._debug('ERROR:com_usb:read', err) raise def write(self, data): @@ -90,24 +81,31 @@ class usb_device(): Write in the device: data """ try: - return self.handle.bulkWrite(ADMIN_MODULE_IN_ENDPOINT, data, TIMEOUT) - except: - if self.debug: - print 'Exception in write usb' + return self.dev.write(ADMIN_MODULE_IN_ENDPOINT, data, USB4ALL_INTERFACE, TIMEOUT) + except Exception, err: + self._debug('ERROR:com_usb:write', err) raise + def get_address(self): + """ + Get unique address for the usb + """ + if self.dev is not None: + return self.dev.address + else: + return None + def get_info(self): """ Get the device info such as manufacturer, etc """ try: - names = self.handle.getString(1, 255) - copy = self.handle.getString(2, 255) - sn = self.handle.getString(3, 255) + names = usb.util.get_string(self.dev, 255, 1, None).encode('ascii') + copy = usb.util.get_string(self.dev, 255, 2, None).encode('ascii') + sn = usb.util.get_string(self.dev, 255, 3, None).encode('ascii') return [names, copy, sn] except Exception, err: - if self.debug: - print 'Exception in get_info', err + self._debug('ERROR:com_usb:get_info', err) raise def find(): @@ -115,13 +113,7 @@ def find(): List all busses and returns a list of baseboards detected """ l = [] - try: - for bus in usb.busses(): - for dev in bus.devices: - if dev.idVendor == USB4ALL_VENDOR and dev.idProduct == USB4ALL_PRODUCT: - l.append(usb_device(dev)) - except Exception, err: - if self.debug: - print 'find gives the error:', err + for b in usb.core.find(find_all=True, idVendor=USB4ALL_VENDOR, idProduct=USB4ALL_PRODUCT): + l.append(usb_device(b)) return l diff --git a/pybot/device.py b/pybot/device.py index 81a9e57..d7e9255 100755..100644 --- a/pybot/device.py +++ b/pybot/device.py @@ -3,6 +3,7 @@ # # Device abstraction for USB4butia # +# Copyright (c) 2012-2013 Alan Aguiar alanjas@hotmail.com # Copyright (c) 2012-2013 Butiá Team butia@fing.edu.uy # Butia is a free and open robotic platform # www.fing.edu.uy/inco/proyectos/butia @@ -26,12 +27,9 @@ NULL_BYTE = 0x00 OPEN_COMMAND = 0x00 CLOSE_COMMAND = 0x01 HEADER_PACKET_SIZE = 0x06 - ADMIN_HANDLER_SEND_COMMAND = 0x00 - OPEN_RESPONSE_PACKET_SIZE = 5 -CLOSE_RESPONSE_PACKET_SIZE = 2 - +CLOSE_RESPONSE_PACKET_SIZE = 5 READ_HEADER_SIZE = 3 MAX_BYTES = 64 @@ -39,116 +37,107 @@ ERROR = -1 class Device(): - def __init__(self, baseboard, name, handler=None): + def __init__(self, baseboard, name, handler=None, func=None): self.baseboard = baseboard self.name = name self.handler = handler + self.shifted = None if not(self.handler == None): - self.handler_tosend = self.handler * 8 - self.functions = {} + self.shifted = self.handler * 8 + self.functions = func self.debug = False - def add_functions(self, func_list): - """ - Add the functions to current device - """ - for f in func_list: - self.functions[f['name']] = f + def _debug(self, message, err=''): + if self.debug: + print message, err - def module_send(self, call, params_length, params): + def send(self, msg): """ Send to the device the specifiy call and parameters """ - if len(params) == 1: - if type(params[0]) == str: - params = to_ord(params[0]) - - send_packet_length = 0x04 + len(params) - - w = [] - w.append(self.handler_tosend) - w.append(send_packet_length) - w.append(NULL_BYTE) - w.append(call) - for p in params: - w.append(p) - + w = [self.shifted, 0x03 + len(msg), NULL_BYTE] + msg self.baseboard.dev.write(w) - def module_read(self): + def read(self, lenght): """ Read the device data """ - raw = self.baseboard.dev.read(MAX_BYTES) - if self.debug: - print 'device:module_rad return', raw - if raw[1] == 5: - if raw[4] == 255: - return -1 - else: - return raw[4] - elif raw[1] == 6: - return raw[4] + raw[5] * 256 - else: - ret = '' - for r in raw[4:]: - if not(r == 0): - ret = ret + chr(r) - return ret + raw = self.baseboard.dev.read(0x03 + lenght) + return raw[3:] def module_open(self): """ Open this device. Return the handler """ - module_name = to_ord(self.name) - module_name.append(0) - - open_packet_length = HEADER_PACKET_SIZE + len(module_name) + module_name = self.to_ord(self.name) + module_name.append(NULL_BYTE) - module_in_endpoint = 0x01 - module_out_endpoint = 0x01 - - w = [] - w.append(ADMIN_HANDLER_SEND_COMMAND) - w.append(open_packet_length) + w = [ADMIN_HANDLER_SEND_COMMAND] + w.append(HEADER_PACKET_SIZE + len(module_name)) w.append(NULL_BYTE) w.append(OPEN_COMMAND) - w.append(module_in_endpoint) - w.append(module_out_endpoint) - w = w + module_name - self.baseboard.dev.write(w) + w.append(0x01) + w.append(0x01) + self.baseboard.dev.write(w + module_name) raw = self.baseboard.dev.read(OPEN_RESPONSE_PACKET_SIZE) - if self.debug: - print 'device:module_open return', raw + self._debug('device:module_open', raw) - h = raw[4] - self.handler = h - self.handler_tosend = self.handler * 8 - return h + if not(raw[4] == 255): + self.handler = raw[4] + self.shifted = self.handler * 8 + return self.handler + else: + self._debug('device:module_open:cannot open module:', self.name) + return 255 + + def module_close(self): + w = [ADMIN_HANDLER_SEND_COMMAND, 0x05, NULL_BYTE, CLOSE_COMMAND, self.handler] + self.baseboard.dev.write(w) + raw = self.baseboard.dev.read(CLOSE_RESPONSE_PACKET_SIZE) + return raw[4] def has_function(self, func): """ Check if this device has func function """ - return self.functions.has_key(func) + return hasattr(self.functions, func) def call_function(self, func, params): """ Call specify func function with params parameters """ - self.module_send(self.functions[func]['call'], self.functions[func]['params'], params) - return self.module_read() - -def to_ord(string): - """ - Useful function to convert characters into ordinal Unicode - """ - s = [] - for l in string: - o = ord(l) - if not(o == 0): - s.append(o) - return s + f = getattr(self.functions, func) + if func == 'send': + return f(self, params) + else: + par = [] + for e in params: + par.append(int(e)) + if func == 'sendPacket': + return f(self, par) + else: + return f(self, *par) + + def to_ord(self, string): + """ + Useful function to convert characters into ordinal Unicode + """ + s = [] + for l in string: + o = ord(l) + if not(o == 0): + s.append(o) + return s + + def to_text(self, raw): + """ + Useful function to convert ordinal Unicode into text + """ + ret = '' + for r in raw: + if not(r == 0): + ret = ret + chr(r) + return ret diff --git a/pybot/drivers/admin.py b/pybot/drivers/admin.py index 5e4e192..01ba4c0 100644 --- a/pybot/drivers/admin.py +++ b/pybot/drivers/admin.py @@ -1,19 +1,26 @@ -RESET = 0xFF +MESSAGE = 0x02 +LOAD = 0x03 +UNLOAD = 0x04 +GET_USER_MODULES_SIZE = 0x05 +GET_USER_MODULES_LINE = 0x06 +BOOT = 0x09 +GET_HANDLER_SIZE = 0x0A +GET_HANDLER_TYPE = 0x0B GET_FIRMWARE_VERSION = 0xFE +RESET = 0xFF + +def getVersion(dev): + dev.send([GET_FIRMWARE_VERSION]) + raw = dev.read(2) + return raw[1] -f1 = { - 'name': 'reset', - 'call': RESET, - 'params': 0, - 'read': 0 -} +def send(dev, data): + msg = [MESSAGE] + dev.to_ord(data[0]) + dev.send(msg) + raw = dev.read(len(msg)) + return dev.to_text(raw[1:]) -f2 = { - 'name': 'getVersion', - 'call': GET_FIRMWARE_VERSION, - 'params': 0, - 'read': 1 -} +def reset(dev): + dev.send([RESET]) -FUNCTIONS = [f1, f2] diff --git a/pybot/drivers/ax.py b/pybot/drivers/ax.py new file mode 100644 index 0000000..eb1762b --- /dev/null +++ b/pybot/drivers/ax.py @@ -0,0 +1,83 @@ + +RD_VERSION = 0x00 +WRITE_INFO = 0x01 +READ_INFO = 0x02 +SEND_RAW = 0x03 + +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + +def writeInfo(dev, motor_id, regstart, value): + msg = [WRITE_INFO, motor_id, regstart, value / 256, value % 256] + dev.send(msg) + raw = dev.read(2) + return raw[1] + +def readInfo(dev, motor_id, regstart, lenght): + msg = [READ_INFO, motor_id, regstart, lenght] + dev.send(msg) + raw = dev.read(3) + if lenght == 1: + return raw[1] + else: + return raw[1] + raw[2] * 256 + +def sendPacket(dev, pack): + wait_resp = len(pack) + 2 + msg = [SEND_RAW, wait_resp] + pack + dev.send(msg) + raw = dev.read(255) + if len(raw) == 1: + return -1 # only opcode o nil + timeout = raw[2] + print 'timeout', timeout + if timeout == 1: + return -1 + size = raw[1] + print "AX12 answer\n:::SIZE = " + str(size) + "\n:::TIMEOUT = " + str(timeout) + msg = '' + for i in range(3,size+3): + msg = msg + str(raw[i]) + ' ' + print ":::MESSAGE\n " + msg + return msg + +def wheelMode(dev, motor_id): + msg = [WRITE_INFO, motor_id, 0x06, 0x00, 0x00] + dev.send(msg) + raw = dev.read(2) + msg = [WRITE_INFO, motor_id, 0x08, 0x00, 0x00] + dev.send(msg) + raw = dev.read(2) + return raw[1] + +def jointMode(dev, motor_id, _min, _max): + msg = [WRITE_INFO, motor_id, 0x06, _min / 256, _min % 256] + dev.send(msg) + raw = dev.read(2) + msg = [WRITE_INFO, motor_id, 0x08, _max / 256, _max % 256] + dev.send(msg) + raw = dev.read(2) + return raw[1] + +def setPosition(dev, motor_id, pos): + msg = [WRITE_INFO, motor_id, 0x1E, pos / 256, pos % 256] + dev.send(msg) + raw = dev.read(2) + return raw[1] + +def getPosition(dev, motor_id): + msg = [READ_INFO, motor_id, 0x24, 2] + dev.send(msg) + raw = dev.read(3) + raw_angle = raw[1] * 256 + raw[2] + return raw_angle * 0.29 + +def setSpeed(dev, motor_id, speed): + #vel = speed * 1.496 + msg = [WRITE_INFO, motor_id, 0x20, speed / 256, speed % 256] + dev.send(msg) + raw = dev.read(2) + return raw[1] + diff --git a/pybot/drivers/butia.py b/pybot/drivers/butia.py index 74f6f0f..20864fd 100644 --- a/pybot/drivers/butia.py +++ b/pybot/drivers/butia.py @@ -2,18 +2,16 @@ RD_VERSION = 0x02 GET_VOLT = 0x03 -f1 = { - 'name': 'read_ver', - 'call': RD_VERSION, - 'params': 0, - 'read': 2 -} +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(2) + return raw[1] -f2 = { - 'name': 'get_volt', - 'call': GET_VOLT, - 'params': 0, - 'read': 2 -} +def getVolt(dev): + dev.send([GET_VOLT]) + raw = dev.read(2) + if raw[1] == 255: + return raw[1] + else: + return raw[1] / 10.0 -FUNCTIONS = [f1, f2] diff --git a/pybot/drivers/hackp.py b/pybot/drivers/hackp.py index 4e9c963..d8aa5c7 100644 --- a/pybot/drivers/hackp.py +++ b/pybot/drivers/hackp.py @@ -7,34 +7,29 @@ WRITE_PORT = 0x04 PORT_IN = 0x05 PORT_OUT = 0x06 - -f1 = { - 'name': 'getVersion', - 'call': RD_VERSION, - 'params': 0, - 'read': 3 -} - -f2 = { - 'name': 'setMode', - 'call': SET_MODE, - 'params': 2, - 'read': 1 -} - -f3 = { - 'name': 'read', - 'call': READ, - 'params': 1, - 'read': 1 -} - -f4 = { - 'name': 'write', - 'call': WRITE, - 'params': 2, - 'read': 1 -} - -FUNCTIONS = [f1, f2, f3, f4] +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + +def setMode(dev, pin, mode): + pin = pin - 1 + msg = [SET_MODE, pin, mode] + dev.send(msg) + raw = dev.read(1) + return raw[0] + +def read(dev, pin): + pin = pin - 1 + msg = [READ, pin] + dev.send(msg) + raw = dev.read(2) + return raw[1] + +def write(dev, pin, value): + pin = pin - 1 + msg = [WRITE, pin, value] + dev.send(msg) + raw = dev.read(1) + return raw[0] diff --git a/pybot/drivers/hotplug/button.py b/pybot/drivers/hotplug/button.py index f714ecf..4bf99ee 100644 --- a/pybot/drivers/hotplug/button.py +++ b/pybot/drivers/hotplug/button.py @@ -1,19 +1,18 @@ RD_VERSION = 0x00 GET_VALUE = 0x01 +ERROR = -1 -f1 = { - 'name': 'getVersion', - 'call': RD_VERSION, - 'params': 0, - 'read': 3 -} +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 -f2 = { - 'name': 'getValue', - 'call': GET_VALUE, - 'params': 0, - 'read': 2 -} +def getValue(dev): + dev.send([GET_VALUE]) + raw = dev.read(2) + if not(raw[1] == 255): + return (1 - raw[1]) + else: + return ERROR -FUNCTIONS = [f1, f2] diff --git a/pybot/drivers/hotplug/distanc.py b/pybot/drivers/hotplug/distanc.py index db7e1c1..6148171 100644 --- a/pybot/drivers/hotplug/distanc.py +++ b/pybot/drivers/hotplug/distanc.py @@ -2,18 +2,13 @@ RD_VERSION = 0x00 GET_VALUE = 0x01 -f1 = { - 'name': 'getVersion', - 'call': RD_VERSION, - 'params': 0, - 'read': 3 -} +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 -f2 = { - 'name': 'getValue', - 'call': GET_VALUE, - 'params': 0, - 'read': 3 -} +def getValue(dev): + dev.send([GET_VALUE]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 -FUNCTIONS = [f1, f2] diff --git a/pybot/drivers/hotplug/grey.py b/pybot/drivers/hotplug/grey.py index db7e1c1..6148171 100644 --- a/pybot/drivers/hotplug/grey.py +++ b/pybot/drivers/hotplug/grey.py @@ -2,18 +2,13 @@ RD_VERSION = 0x00 GET_VALUE = 0x01 -f1 = { - 'name': 'getVersion', - 'call': RD_VERSION, - 'params': 0, - 'read': 3 -} +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 -f2 = { - 'name': 'getValue', - 'call': GET_VALUE, - 'params': 0, - 'read': 3 -} +def getValue(dev): + dev.send([GET_VALUE]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 -FUNCTIONS = [f1, f2] diff --git a/pybot/drivers/hotplug/led.py b/pybot/drivers/hotplug/led.py index 0703d54..fde6b3e 100644 --- a/pybot/drivers/hotplug/led.py +++ b/pybot/drivers/hotplug/led.py @@ -2,18 +2,14 @@ RD_VERSION = 0x00 TURN = 0x01 -f1 = { - 'name': 'getVersion', - 'call': RD_VERSION, - 'params': 0, - 'read': 3 -} +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 -f2 = { - 'name': 'turn', - 'call': TURN, - 'params': 1, - 'read': 1 -} +def turn(dev, on): + msg = [TURN, on] + dev.send(msg) + raw = dev.read(1) + return raw[0] -FUNCTIONS = [f1, f2] diff --git a/pybot/drivers/hotplug/light.py b/pybot/drivers/hotplug/light.py index db7e1c1..dbcb71a 100644 --- a/pybot/drivers/hotplug/light.py +++ b/pybot/drivers/hotplug/light.py @@ -2,18 +2,16 @@ RD_VERSION = 0x00 GET_VALUE = 0x01 -f1 = { - 'name': 'getVersion', - 'call': RD_VERSION, - 'params': 0, - 'read': 3 -} +VCC = 65536 -f2 = { - 'name': 'getValue', - 'call': GET_VALUE, - 'params': 0, - 'read': 3 -} +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + +def getValue(dev): + dev.send([GET_VALUE]) + raw = dev.read(3) + val = raw[1] + raw[2] * 256 + return (VCC - val) -FUNCTIONS = [f1, f2] diff --git a/pybot/drivers/hotplug/modActA.py b/pybot/drivers/hotplug/modActA.py new file mode 100644 index 0000000..fde6b3e --- /dev/null +++ b/pybot/drivers/hotplug/modActA.py @@ -0,0 +1,15 @@ + +RD_VERSION = 0x00 +TURN = 0x01 + +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + +def turn(dev, on): + msg = [TURN, on] + dev.send(msg) + raw = dev.read(1) + return raw[0] + diff --git a/pybot/drivers/hotplug/modActB.py b/pybot/drivers/hotplug/modActB.py new file mode 100644 index 0000000..fde6b3e --- /dev/null +++ b/pybot/drivers/hotplug/modActB.py @@ -0,0 +1,15 @@ + +RD_VERSION = 0x00 +TURN = 0x01 + +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + +def turn(dev, on): + msg = [TURN, on] + dev.send(msg) + raw = dev.read(1) + return raw[0] + diff --git a/pybot/drivers/hotplug/modActC.py b/pybot/drivers/hotplug/modActC.py new file mode 100644 index 0000000..fde6b3e --- /dev/null +++ b/pybot/drivers/hotplug/modActC.py @@ -0,0 +1,15 @@ + +RD_VERSION = 0x00 +TURN = 0x01 + +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + +def turn(dev, on): + msg = [TURN, on] + dev.send(msg) + raw = dev.read(1) + return raw[0] + diff --git a/pybot/drivers/hotplug/modSenA.py b/pybot/drivers/hotplug/modSenA.py new file mode 100644 index 0000000..6148171 --- /dev/null +++ b/pybot/drivers/hotplug/modSenA.py @@ -0,0 +1,14 @@ + +RD_VERSION = 0x00 +GET_VALUE = 0x01 + +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + +def getValue(dev): + dev.send([GET_VALUE]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + diff --git a/pybot/drivers/hotplug/modSenB.py b/pybot/drivers/hotplug/modSenB.py new file mode 100644 index 0000000..6148171 --- /dev/null +++ b/pybot/drivers/hotplug/modSenB.py @@ -0,0 +1,14 @@ + +RD_VERSION = 0x00 +GET_VALUE = 0x01 + +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + +def getValue(dev): + dev.send([GET_VALUE]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + diff --git a/pybot/drivers/hotplug/modSenC.py b/pybot/drivers/hotplug/modSenC.py new file mode 100644 index 0000000..6148171 --- /dev/null +++ b/pybot/drivers/hotplug/modSenC.py @@ -0,0 +1,14 @@ + +RD_VERSION = 0x00 +GET_VALUE = 0x01 + +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + +def getValue(dev): + dev.send([GET_VALUE]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + diff --git a/pybot/drivers/hotplug/res.py b/pybot/drivers/hotplug/res.py index db7e1c1..0dc43a9 100644 --- a/pybot/drivers/hotplug/res.py +++ b/pybot/drivers/hotplug/res.py @@ -2,18 +2,16 @@ RD_VERSION = 0x00 GET_VALUE = 0x01 -f1 = { - 'name': 'getVersion', - 'call': RD_VERSION, - 'params': 0, - 'read': 3 -} +VCC = 65536 -f2 = { - 'name': 'getValue', - 'call': GET_VALUE, - 'params': 0, - 'read': 3 -} +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + +def getValue(dev): + dev.send([GET_VALUE]) + raw = dev.read(3) + val = raw[1] + raw[2] * 256 + return val * 6800.0 / (VCC - val) -FUNCTIONS = [f1, f2] diff --git a/pybot/drivers/hotplug/temp.py b/pybot/drivers/hotplug/temp.py new file mode 100644 index 0000000..7fa55a1 --- /dev/null +++ b/pybot/drivers/hotplug/temp.py @@ -0,0 +1,19 @@ + +import math + +RD_VERSION = 0x00 +GET_VALUE = 0x01 + +VCC = 65536 + +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + +def getValue(dev): + dev.send([GET_VALUE]) + raw = dev.read(3) + volt = (raw[1] + raw[2] * 256) * 5.0 / VCC + return math.floor(volt * 1000.0) / 10 + diff --git a/pybot/drivers/hotplug/volt.py b/pybot/drivers/hotplug/volt.py index db7e1c1..10866a6 100644 --- a/pybot/drivers/hotplug/volt.py +++ b/pybot/drivers/hotplug/volt.py @@ -1,19 +1,19 @@ +import math + RD_VERSION = 0x00 GET_VALUE = 0x01 -f1 = { - 'name': 'getVersion', - 'call': RD_VERSION, - 'params': 0, - 'read': 3 -} +VCC = 65536 + +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 -f2 = { - 'name': 'getValue', - 'call': GET_VALUE, - 'params': 0, - 'read': 3 -} +def getValue(dev): + dev.send([GET_VALUE]) + raw = dev.read(3) + volt = (raw[1] + raw[2] * 256) * 5.0 / VCC + return math.floor(volt * 1000.0) / 1000 -FUNCTIONS = [f1, f2] diff --git a/pybot/drivers/lback.py b/pybot/drivers/lback.py index 859eedb..0d036f8 100644 --- a/pybot/drivers/lback.py +++ b/pybot/drivers/lback.py @@ -2,18 +2,14 @@ RD_VERSION = 0x00 SEND_DATA = 0x01 -f1 = { - 'name': 'getVersion', - 'call': RD_VERSION, - 'params': 0, - 'read': 3 -} +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 -f2 = { - 'name': 'send', - 'call': SEND_DATA, - 'params': 1, - 'read': 1 -} +def send(dev, data): + msg = [SEND_DATA] + dev.to_ord(data[0]) + dev.send(msg) + raw = dev.read(len(msg)) + return dev.to_text(raw[1:]) -FUNCTIONS = [f1, f2] diff --git a/pybot/drivers/motors.py b/pybot/drivers/motors.py index 02421d9..e62192a 100644 --- a/pybot/drivers/motors.py +++ b/pybot/drivers/motors.py @@ -2,26 +2,33 @@ RD_VERSION = 0x00 SET_VEL_2MTR = 0x01 SET_VEL_MTR = 0x02 +TEST_MOTORS = 0x03 +GET_TYPE = 0x04 -f1 = { - 'name': 'getVersion', - 'call': RD_VERSION, - 'params': 0, - 'read': 3 -} +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 -f2 = { - 'name': 'setvel2mtr', - 'call': SET_VEL_2MTR, - 'params': 6, - 'read': 1 -} +def setvel2mtr(dev, sentido1, vel1, sentido2, vel2): + msg = [SET_VEL_2MTR, sentido1, vel1 / 256, vel1 % 256, sentido2, vel2 / 256, vel2 % 256] + dev.send(msg) + raw = dev.read(1) + return raw[0] -f3 = { - 'name': 'setvelmtr', - 'call': SET_VEL_MTR, - 'params': 4, - 'read': 1 -} +def setvelmtr(dev, motor_id, sentido, vel): + msg = [SET_VEL_MTR, motor_id, sentido, vel / 256, vel % 256] + dev.send(msg) + raw = dev.read(1) + return raw[0] + +def testMotors(dev): + dev.send([TEST_MOTORS]) + raw = dev.read(1) + return raw[0] + +def getType(dev): + dev.send([GET_TYPE]) + raw = dev.read(2) + return raw[1] -FUNCTIONS = [f1, f2, f3] diff --git a/pybot/drivers/pnp.py b/pybot/drivers/pnp.py index 3457126..24d6dcd 100644 --- a/pybot/drivers/pnp.py +++ b/pybot/drivers/pnp.py @@ -1,11 +1,8 @@ RD_VERSION = 0x00 -f1 = { - 'name': 'getVersion', - 'call': RD_VERSION, - 'params': 0, - 'read': 3 -} +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 -FUNCTIONS = [f1] diff --git a/pybot/drivers/shld_cc.py b/pybot/drivers/shld_cc.py new file mode 100644 index 0000000..35b94ae --- /dev/null +++ b/pybot/drivers/shld_cc.py @@ -0,0 +1,15 @@ + +RD_VERSION = 0x00 +SET_VEL_2MTR = 0x01 + +def getVersion(dev): + dev.send([RD_VERSION]) + raw = dev.read(3) + return raw[1] + raw[2] * 256 + +def setvel2mtr(dev, sentido1, vel1, sentido2, vel2): + msg = [SET_VEL_2MTR, sentido1, vel1 / 256, vel1 % 256, sentido2, vel2 / 256, vel2 % 256] + dev.send(msg) + raw = dev.read(1) + return raw[0] + diff --git a/pybot/functions.py b/pybot/functions.py new file mode 100644 index 0000000..71dbf42 --- /dev/null +++ b/pybot/functions.py @@ -0,0 +1,254 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- +# +# Abstract class with common functions +# +# Copyright (c) 2012-2013 Alan Aguiar alanjas@hotmail.com +# Copyright (c) 2012-2013 Butiá Team butia@fing.edu.uy +# Butia is a free and open robotic platform +# www.fing.edu.uy/inco/proyectos/butia +# Facultad de Ingeniería - Universidad de la República - Uruguay +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +class ButiaFunctions: + + def isPresent(self, module_name): + """ + Check if module: module_name is present + """ + module_list = self.get_modules_list() + return (module_name in module_list) + + def loopBack(self, data, board='0'): + """ + LoopBack command: send data to the board and get the result. If all is ok + the return must be exactly of the data parameter + """ + msg = [str(data)] + return self.callModule('lback', str(board), '0', 'send', msg, ret_type=str) + + ############################## Movement calls ############################## + + def set2MotorSpeed(self, leftSense='0', leftSpeed='0', rightSense='0', rightSpeed='0', board='0'): + """ + Set the speed of 2 motors. The sense is 0 or 1, and the speed is + between 0 and 1023 + """ + msg = [str(leftSense), str(leftSpeed), str(rightSense), str(rightSpeed)] + return self.callModule('motors', str(board), '0', 'setvel2mtr', msg) + + def setMotorSpeed(self, idMotor='0', sense='0', speed='0', board='0'): + """ + Set the speed of one motor. idMotor = 0 for left motor and 1 for the + right motor. The sense is 0 or 1, and the speed is between 0 and 1023 + """ + msg = [str(idMotor), str(sense), str(speed)] + return self.callModule('motors', str(board), '0', 'setvelmtr', msg) + + def getMotorType(self, board='0'): + """ + If AX-12 motors present returns 1. If there are a shield "cc" returns 2 + """ + return self.callModule('motors', str(board), '0', 'getType') + + ##################### Operations for ax.lua driver ######################### + + def writeInfo(self, idMotor, regstart, value, board='0'): + """ + Writes the motor: idMotor in the registry: regstart with value: value + """ + msg = [str(idMotor), str(regstart), str(value)] + return self.callModule('ax', str(board), '0', 'writeInfo', msg) + + def readInfo(self, idMotor, regstart, length='1', board='0'): + """ + Reads the motor: idMotor in the registry: regstart + """ + msg = [str(idMotor), str(regstart), str(length)] + return self.callModule('ax', str(board), '0', 'writeInfo', msg) + + def sendPacket(self, msg, board='0'): + """ + Send a raw packet to ax module + """ + msg_s = [str(i) for i in msg] + return self.callModule('ax', str(board), '0', 'sendPacket', msg_s, ret_type=str) + + def wheelMode(self, idMotor='0', board='0'): + """ + Sets the motor: idMotor in wheel mode (continuos rotation) + """ + msg = [str(idMotor)] + return self.callModule('ax', str(board), '0', 'wheelMode', msg) + + def jointMode(self, idMotor='0', _min='0', _max='1023', board='0'): + """ + Sets the motor: idMotor in servo mode + """ + msg = [str(idMotor), str(_min), str(_max)] + return self.callModule('ax', str(board), '0', 'jointMode', msg) + + def setPosition(self, idMotor='0', pos='0', board='0'): + """ + Sets the position: pos of the motor: idMotor + """ + msg = [str(idMotor), str(pos)] + return self.callModule('ax', str(board), '0', 'setPosition', msg) + + def getPosition(self, idMotor='0', board='0'): + """ + Gets the position of motor: idMotor + """ + msg = [str(idMotor)] + return self.callModule('ax', str(board), '0', 'getPosition', msg) + + def setSpeed(self, idMotor='0', speed='0', board='0'): + """ + Set the speed: speed to the motor: idMotor + """ + msg = [str(idMotor), str(speed)] + return self.callModule('ax', str(board), '0', 'setSpeed', msg) + + ############################### General calls ############################## + + def getBatteryCharge(self, board='0'): + """ + Gets the battery level charge + """ + return self.callModule('butia', str(board), '0', 'getVolt', ret_type=float) + + def getVersion(self, board='0'): + """ + Gets the version of Butiá module. 22 for new version + """ + return self.callModule('butia', str(board), '0', 'getVersion') + + def getFirmwareVersion(self, board='0'): + """ + Gets the version of the Firmware + """ + return self.callModule('admin', str(board), '0', 'getVersion') + + ############################### Sensors calls ############################### + + def getButton(self, port, board='0'): + """ + Gets the value of the button connected in port + """ + return self.callModule('button', str(board), str(port), 'getValue') + + def getLight(self, port, board='0'): + """ + Gets the value of the light sensor connected in port + """ + return self.callModule('light', str(board), str(port), 'getValue') + + def getDistance(self, port, board='0'): + """ + Gets the value of the distance sensor connected in port + """ + return self.callModule('distanc', str(board), str(port), 'getValue') + + def getGray(self, port, board='0'): + """ + Gets the value of the gray sensor connected in port + """ + return self.callModule('grey', str(board), str(port), 'getValue') + + def getResistance(self, port, board='0'): + """ + Gets the value of the resistance sensor connected in port + """ + return self.callModule('res', str(board), str(port), 'getValue', ret_type=float) + + def getVoltage(self, port, board='0'): + """ + Gets the value of the voltage sensor connected in port + """ + return self.callModule('volt', str(board), str(port), 'getValue', ret_type=float) + + def getTemperature(self, port, board='0'): + """ + Gets the value of the temperature sensor connected in port + """ + return self.callModule('temp', str(board), str(port), 'getValue', ret_type=float) + + def setLed(self, port, on_off, board='0'): + """ + Sets on or off the LED connected in port (0 is off, 1 is on) + """ + return self.callModule('led', str(board), str(port), 'turn', [str(on_off)]) + + ################################ Extras ################################ + + def modeHack(self, pin, mode, board='0'): + """ + Sets the mode of hack pin. If mode 0 = input, mode 1 = output + """ + msg = [str(pin), str(mode)] + return self.callModule('hackp', str(board), '0', 'setMode', msg) + + def setHack(self, pin, value, board='0'): + """ + Sets the value of hack pin configured as output. Value is 0 or 1 + """ + msg = [str(pin), str(value)] + return self.callModule('hackp', str(board), '0', 'write', msg) + + def getHack(self, pin, board='0'): + """ + Gets the value of hack pin configured as input. Returns 0 or 1 + """ + return self.callModule('hackp', str(board), '0', 'read', [str(pin)]) + + ############################# Generic modules ############################# + + def getModuleA(self, port, board='0'): + """ + Gets the value of the generic sensor ModuleA connected in port + """ + return self.callModule('moduleA', str(board), str(port), 'getValue') + + def getModuleB(self, port, board='0'): + """ + Gets the value of the generic sensor ModuleB connected in port + """ + return self.callModule('moduleB', str(board), str(port), 'getValue') + + def getModuleC(self, port, board='0'): + """ + Gets the value of the generic sensor ModuleC connected in port + """ + return self.callModule('moduleC', str(board), str(port), 'getValue') + + def setModuleA(self, port, on_off, board='0'): + """ + Sets on or off the generic module A + """ + return self.callModule('modSenA', str(board), str(port), 'turn', [str(on_off)]) + + def setModuleB(self, port, on_off, board='0'): + """ + Sets on or off the generic module B + """ + return self.callModule('modSenB', str(board), str(port), 'turn', [str(on_off)]) + + def setModuleC(self, port, on_off, board='0'): + """ + Sets on or off the generic module C + """ + return self.callModule('modSenC', str(board), str(port), 'turn', [str(on_off)]) + diff --git a/pybot/pybot_client.py b/pybot/pybot_client.py new file mode 100755 index 0000000..4e53f1e --- /dev/null +++ b/pybot/pybot_client.py @@ -0,0 +1,195 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- +# +# Client for pybot_server +# +# Copyright (c) 2012-2013 Alan Aguiar alanjas@hotmail.com +# Copyright (c) 2009-2013 Butiá Team butia@fing.edu.uy +# Butia is a free and open robotic platform +# www.fing.edu.uy/inco/proyectos/butia +# Facultad de Ingeniería - Universidad de la República - Uruguay +# +# Implements abstractions for the comunications with the bobot-server +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +import socket +import threading +import errno +from functions import ButiaFunctions + +ERROR = -1 + +PYBOT_HOST = 'localhost' +PYBOT_PORT = 2009 + +class robot(ButiaFunctions): + + def __init__(self, host=PYBOT_HOST, port=PYBOT_PORT, auto_connect=True): + """ + init the robot class + """ + self._lock = threading.Lock() + self._host = host + self._port = port + self._client = None + if auto_connect: + self.reconnect() + + def _doCommand(self, msg, ret_type = str): + """ + Executes a command in butia. + @param msg message to be executed + """ + msg = msg + '\n' + ret = ERROR + self._lock.acquire() + try: + self._client.send(msg) + ret = self._client.recv(1024) + ret = ret[:-1] + except Exception, e: + if hasattr(e, 'errno'): + if e.errno == errno.EPIPE: + self.reconnect() + self._lock.release() + return ERROR + try: + ret = ret_type(ret) + except: + ret = ERROR + self._lock.release() + return ret + + def reconnect(self): + """ + connect o reconnect the bobot + """ + self.close() + try: + self._client = socket.socket() + self._client.connect((self._host, self._port)) + except: + return ERROR + return 0 + + def refresh(self): + """ + ask bobot for refresh is state of devices connected + """ + self._doCommand('REFRESH') + + def close(self): + """ + close the comunication with pybot + """ + try: + self._client.close() + self._client = None + except: + return ERROR + return 0 + + def callModule(self, modulename, board_number, number, function, params = [], ret_type = int): + """ + call the module 'modulename' + """ + msg = 'CALL ' + modulename + '@' + str(board_number) + ':' + str(number) + ' ' + function + if not(params == []): + msg = msg + ' ' + ' '.join(params) + return self._doCommand(msg, ret_type) + + def closeService(self): + """ + Close bobot service + """ + return self._doCommand('QUIT') + + def getButiaCount(self): + """ + Gets the number of boards detected + """ + return self._doCommand('BUTIA_COUNT', int) + + def getModulesList(self): + """ + returns a list of modules + """ + ret = self._doCommand('LIST') + if (ret == ERROR) or (ret == ''): + return [] + else: + return ret.split(',') + + def getListi(self, board_number=0): + """ + returns a list of instanciables modules + """ + ret = self._doCommand('LISTI ' + str(board_number)) + if (ret == ERROR) or (ret == ''): + return [] + else: + return ret.split(',') + + def _split_module(self, mbn): + """ + Split a modulename: module@board:port to (number, modulename, board) + """ + board = '0' + number = '0' + if mbn.count('@') > 0: + modulename, bn = mbn.split('@') + if bn.count(':') > 0: + board, number = bn.split(':') + else: + board = bn + else: + if mbn.count(':') > 0: + modulename, number = mbn.split(':') + else: + modulename = mbn + return (number, modulename, board) + + def describe(self, mod): + """ + Describe the functions of a modulename + """ + split = self._split_module(mod) + mod = split[1] + ret = self._doCommand('DESCRIBE ' + mod) + return ret + + def moduleOpen(self, mod): + """ + Open the module mod + """ + return self._doCommand('OPEN ' + mod, int) + + def moduleClose(self, mod): + """ + Close the module mod + """ + return self._doCommand('CLOSE ' + mod, int) + +if __name__ == "__main__": + c = robot() + run = True + while run: + m = raw_input("> ") + ret = c._doCommand(m) + print ret + if m == "QUIT": + run = False + c.close() + diff --git a/pybot/pybot_server.py b/pybot/pybot_server.py index 0b6000d..1c13d06 100755 --- a/pybot/pybot_server.py +++ b/pybot/pybot_server.py @@ -3,6 +3,7 @@ # # Pybot server # +# Copyright (c) 2012-2013 Alan Aguiar alanjas@hotmail.com # Copyright (c) 2012-2013 Butiá Team butia@fing.edu.uy # Butia is a free and open robotic platform # www.fing.edu.uy/inco/proyectos/butia @@ -22,44 +23,40 @@ # Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA import sys +import imp import select import socket import usb4butia +import com_chotox argv = sys.argv[:] -PYBOT_HOST = 'localhost' PYBOT_PORT = 2009 BUFSIZ = 1024 MAX_CLIENTS = 4 - class Server(): - def __init__(self, debug=False): + def __init__(self, debug=False, chotox=False): self.debug = debug + self.run = True + self.comms = imp.load_source('server_functions', 'server_functions.py') self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - self.socket.bind((PYBOT_HOST, PYBOT_PORT)) + self.socket.bind(("", PYBOT_PORT)) self.socket.listen(MAX_CLIENTS) self.clients = {} - self.robot = usb4butia.USB4Butia(self.debug) - - def call_aux(self, modulename, board_number, number, function, params): - if modulename == 'lback': - par = params + self.chotox_mode = chotox + if self.chotox_mode: + self.robot = com_chotox.Chotox(debug=self.debug) else: - par = [] - for e in params: - par.append(int(e)) - return self.robot.callModule(modulename, board_number, number, function, par) + self.robot = usb4butia.USB4Butia(debug=self.debug) def init_server(self): inputs = [self.socket] - run = True - while run: + while self.run: try: inputready,outputready,exceptready = select.select(inputs, [], []) @@ -77,49 +74,16 @@ class Server(): data = s.recv(BUFSIZ) if data: result = '' - # remove end line characters if become from telnet + r = data.replace('\r', '') r = r.replace('\n', '') r = r.split(' ') - #print 'split', r - if len(r) > 0: - if r[0] == 'QUIT': - result = 'BYE' - run = False - elif r[0] == 'CLIENTS': - first = True - for c in self.clients: - addr = self.clients[c] - if first: - result = result + str(addr[0]) + ', ' + str(addr[1]) - first = False - else: - result = result + '\n' + str(addr[0]) + ', ' + str(addr[1]) - elif r[0] == 'LIST': - l = self.robot.get_modules_list() - result = ','.join(l) - elif r[0] == 'REFRESH': - self.robot.refresh() - elif r[0] == 'BUTIA_COUNT': - result = self.robot.get_butia_count() - elif r[0] == 'CALL': - if len(r) >= 3: - board = 0 - number = 0 - mbn = r[1] - if mbn.count('@') > 0: - modulename, bn = mbn.split('@') - board, number = bn.split(':') - else: - if mbn.count(':') > 0: - modulename, number = mbn.split(':') - else: - modulename = mbn - function = r[2] - par = r[3:] - result = self.call_aux(modulename, int(board), int(number), function, par) + com = r[0] + if hasattr(self.comms, com): + f = getattr(self.comms, com) + result = f(self, r) result = str(result) try: @@ -136,11 +100,9 @@ class Server(): self.socket.close() self.robot.close() - if __name__ == "__main__": - if 'DEBUG' in argv: - s = Server(True) - else: - s = Server() + chotox = 'chotox' in argv + debug = 'DEBUG' in argv + s = Server(debug, chotox) s.init_server() diff --git a/pybot/server_functions.py b/pybot/server_functions.py new file mode 100644 index 0000000..90bdc66 --- /dev/null +++ b/pybot/server_functions.py @@ -0,0 +1,89 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- +# +# Pybot server functions +# +# Copyright (c) 2012-2013 Alan Aguiar alanjas@hotmail.com +# Copyright (c) 2012-2013 Butiá Team butia@fing.edu.uy +# Butia is a free and open robotic platform +# www.fing.edu.uy/inco/proyectos/butia +# Facultad de Ingeniería - Universidad de la República - Uruguay +# +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + + +def QUIT(parent, r): + parent.run = False + return 'BYE' + +def REFRESH(parent, r): + parent.robot.refresh() + return '' + +def OPEN(parent, r): + if len(r) == 2: + module = r[1] + return parent.robot.moduleOpen(module) + return '' + +def CLOSE(parent, r): + if len(r) == 2: + module = r[1] + return parent.robot.moduleClose(module) + return '' + +def DESCRIBE(parent, r): + if len(r) == 2: + module = r[1] + return parent.robot.describe(module) + return '' + +def BUTIA_COUNT(parent, r): + return parent.robot.getButiaCount() + +def LISTI(parent, r): + board = 0 + if len(r) >= 2: + board = r[1] + l = parent.robot.getListi(board) + return ','.join(l) + +def LIST(parent, r): + l = parent.robot.getModulesList() + return ','.join(l) + +def CLIENTS(parent, r): + l = [] + for c in parent.clients: + addr = parent.clients[c] + l.append(str(addr[0]) + ', ' + str(addr[1])) + return '\n'.join(l) + +def CALL(parent, r): + if len(r) >= 3: + split = parent.robot._split_module(r[1]) + return parent.robot.callModule(split[1], split[2], split[0], r[2], r[3:]) + return '' + +def HELP(parent, r): + l = [] + flag = True + a = dir(parent.comms) + for p in a: + if p == '__builtins__': + flag = False + if flag: + l.append(p) + return ', '.join(l) + diff --git a/pybot/usb/__init__.py b/pybot/usb/__init__.py index 8909cf2..6dc5396 100644 --- a/pybot/usb/__init__.py +++ b/pybot/usb/__init__.py @@ -1,8 +1,8 @@ -# Copyright (C) 2009-2011 Wander Lairson Costa -# +# Copyright (C) 2009-2013 Wander Lairson Costa +# # The following terms apply to all files associated # with the software unless explicitly disclaimed in individual files. -# +# # The authors hereby grant permission to use, copy, modify, distribute, # and license this software and its documentation for any purpose, provided # that existing copyright notices are retained in all copies and that this @@ -12,13 +12,13 @@ # and need not follow the licensing terms described here, provided that # the new terms are clearly indicated on the first page of each file where # they apply. -# +# # IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY # FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES # ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY # DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -# +# # THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES, # INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT. THIS SOFTWARE @@ -43,13 +43,18 @@ import os __author__ = 'Wander Lairson Costa' +# Use Semantic Versioning, http://semver.org/ +version_info = (1, 0, 0, 'a4') +__version__ = '%d.%d.%d%s' % version_info + + __all__ = ['legacy', 'core', 'backend', 'util'] def _setup_log(): from usb import _debug logger = logging.getLogger('usb') - debug_level = os.getenv('PYUSB_DEBUG_LEVEL') + debug_level = os.getenv('PYUSB_DEBUG') if debug_level is not None: _debug.enable_tracing(True) diff --git a/pybot/usb/_debug.py b/pybot/usb/_debug.py index 13b0ced..8b1f910 100644 --- a/pybot/usb/_debug.py +++ b/pybot/usb/_debug.py @@ -1,4 +1,4 @@ -# Copyright (C) 2009-2011 Wander Lairson Costa +# Copyright (C) 2009-2013 Wander Lairson Costa # # The following terms apply to all files associated # with the software unless explicitly disclaimed in individual files. diff --git a/pybot/usb/_interop.py b/pybot/usb/_interop.py index d6d0a6c..93b8f04 100644 --- a/pybot/usb/_interop.py +++ b/pybot/usb/_interop.py @@ -1,4 +1,4 @@ -# Copyright (C) 2009-2011 Wander Lairson Costa +# Copyright (C) 2009-2013 Wander Lairson Costa # # The following terms apply to all files associated # with the software unless explicitly disclaimed in individual files. diff --git a/pybot/usb/backend/__init__.py b/pybot/usb/backend/__init__.py index 67ee00f..e08d7bc 100644 --- a/pybot/usb/backend/__init__.py +++ b/pybot/usb/backend/__init__.py @@ -1,4 +1,4 @@ -# Copyright (C) 2009-2011 Wander Lairson Costa +# Copyright (C) 2009-2013 Wander Lairson Costa # # The following terms apply to all files associated # with the software unless explicitly disclaimed in individual files. diff --git a/pybot/usb/backend/libusb0.py b/pybot/usb/backend/libusb0.py index a9271c6..8738335 100644 --- a/pybot/usb/backend/libusb0.py +++ b/pybot/usb/backend/libusb0.py @@ -1,8 +1,8 @@ -# Copyright (C) 2009-2011 Wander Lairson Costa -# +# Copyright (C) 2009-2013 Wander Lairson Costa +# # The following terms apply to all files associated # with the software unless explicitly disclaimed in individual files. -# +# # The authors hereby grant permission to use, copy, modify, distribute, # and license this software and its documentation for any purpose, provided # that existing copyright notices are retained in all copies and that this @@ -12,13 +12,13 @@ # and need not follow the licensing terms described here, provided that # the new terms are clearly indicated on the first page of each file where # they apply. -# +# # IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY # FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES # ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY # DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -# +# # THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES, # INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT. THIS SOFTWARE @@ -183,7 +183,7 @@ class _DeviceDescriptor: self.bNumConfigurations = desc.bNumConfigurations self.address = dev.devnum self.bus = dev.bus[0].location - + self.port_number = None _lib = None @@ -191,6 +191,10 @@ def _load_library(): if sys.platform != 'cygwin': candidates = ('usb-0.1', 'usb', 'libusb0') for candidate in candidates: + # Workaround for CPython 3.3 issue#16283 / pyusb #14 + if sys.platform == 'win32': + candidate = candidate + '.dll' + libname = ctypes.util.find_library(candidate) if libname is not None: break else: @@ -459,7 +463,7 @@ class _LibUSB(usb.backend.IBackend): 1, 100 )[0] - + @methodtrace(_logger) def claim_interface(self, dev_handle, intf): @@ -561,7 +565,7 @@ class _LibUSB(usb.backend.IBackend): ))) def __read(self, fn, dev_handle, ep, intf, size, timeout): - data = _interop.as_array((0,) * size) + data = _interop.as_array('\x00' * size) address, length = data.buffer_info() length *= data.itemsize ret = int(_check(fn( diff --git a/pybot/usb/backend/libusb1.py b/pybot/usb/backend/libusb1.py index bd7d16f..5396092 100644 --- a/pybot/usb/backend/libusb1.py +++ b/pybot/usb/backend/libusb1.py @@ -1,8 +1,8 @@ -# Copyright (C) 2009-2011 Wander Lairson Costa -# +# Copyright (C) 2009-2013 Wander Lairson Costa +# # The following terms apply to all files associated # with the software unless explicitly disclaimed in individual files. -# +# # The authors hereby grant permission to use, copy, modify, distribute, # and license this software and its documentation for any purpose, provided # that existing copyright notices are retained in all copies and that this @@ -12,13 +12,13 @@ # and need not follow the licensing terms described here, provided that # the new terms are clearly indicated on the first page of each file where # they apply. -# +# # IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY # FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES # ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY # DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -# +# # THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES, # INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT. THIS SOFTWARE @@ -34,6 +34,8 @@ import logging from usb._debug import methodtrace import usb._interop as _interop import errno +import math +from usb.core import USBError __author__ = 'Wander Lairson Costa' @@ -53,12 +55,29 @@ __all__ = [ 'LIBUSB_ERROR_NO_MEM', 'LIBUSB_ERROR_NOT_SUPPORTED', 'LIBUSB_ERROR_OTHER' + 'LIBUSB_TRANSFER_COMPLETED', + 'LIBUSB_TRANSFER_ERROR', + 'LIBUSB_TRANSFER_TIMED_OUT', + 'LIBUSB_TRANSFER_CANCELLED', + 'LIBUSB_TRANSFER_STALL', + 'LIBUSB_TRANSFER_NO_DEVICE', + 'LIBUSB_TRANSFER_OVERFLOW' ] _logger = logging.getLogger('usb.backend.libusb1') # libusb.h +# transfer_type codes +# Control endpoint +_LIBUSB_TRANSFER_TYPE_CONTROL = 0, +# Isochronous endpoint +_LIBUSB_TRANSFER_TYPE_ISOCHRONOUS = 1 +# Bulk endpoint +_LIBUSB_TRANSFER_TYPE_BULK = 2 +# Interrupt endpoint +_LIBUSB_TRANSFER_TYPE_INTERRUPT = 3 + # return codes LIBUSB_SUCCESS = 0 @@ -96,7 +115,7 @@ _str_error = { # map return code to errno values _libusb_errno = { - LIBUSB_SUCCESS:None, + 0:None, LIBUSB_ERROR_IO:errno.__dict__.get('EIO', None), LIBUSB_ERROR_INVALID_PARAM:errno.__dict__.get('EINVAL', None), LIBUSB_ERROR_ACCESS:errno.__dict__.get('EACCES', None), @@ -112,6 +131,41 @@ _libusb_errno = { LIBUSB_ERROR_OTHER:None } +# Transfer status codes: +# Note that this does not indicate +# that the entire amount of requested data was transferred. +LIBUSB_TRANSFER_COMPLETED = 0 +LIBUSB_TRANSFER_ERROR = 1 +LIBUSB_TRANSFER_TIMED_OUT = 2 +LIBUSB_TRANSFER_CANCELLED = 3 +LIBUSB_TRANSFER_STALL = 4 +LIBUSB_TRANSFER_NO_DEVICE = 5 +LIBUSB_TRANSFER_OVERFLOW = 6 + +# map return codes to strings +_str_transfer_error = { + LIBUSB_TRANSFER_COMPLETED:'Success (no error)', + LIBUSB_TRANSFER_ERROR:'Transfer failed', + LIBUSB_TRANSFER_TIMED_OUT:'Transfer timed out', + LIBUSB_TRANSFER_CANCELLED:'Transfer was cancelled', + LIBUSB_TRANSFER_STALL:'For bulk/interrupt endpoints: halt condition '\ + 'detected (endpoint stalled). For control '\ + 'endpoints: control request not supported.', + LIBUSB_TRANSFER_NO_DEVICE:'Device was disconnected', + LIBUSB_TRANSFER_OVERFLOW:'Device sent more data than requested' +} + +# map transfer codes to errno codes +_transfer_errno = { + LIBUSB_TRANSFER_COMPLETED:0, + LIBUSB_TRANSFER_ERROR:errno.__dict__.get('EIO', None), + LIBUSB_TRANSFER_TIMED_OUT:errno.__dict__.get('ETIMEDOUT', None), + LIBUSB_TRANSFER_CANCELLED:errno.__dict__.get('EAGAIN', None), + LIBUSB_TRANSFER_STALL:errno.__dict__.get('EIO', None), + LIBUSB_TRANSFER_NO_DEVICE:errno.__dict__.get('ENODEV', None), + LIBUSB_TRANSFER_OVERFLOW:errno.__dict__.get('EOVERFLOW', None) +} + # Data structures class _libusb_endpoint_descriptor(Structure): @@ -173,15 +227,49 @@ class _libusb_device_descriptor(Structure): ('iSerialNumber', c_uint8), ('bNumConfigurations', c_uint8)] -_lib = None -_init = None + +# Isochronous packet descriptor. +class _libusb_iso_packet_descriptor(Structure): + _fields_ = [('length', c_uint), + ('actual_length', c_uint), + ('status', c_int)] # enum libusb_transfer_status _libusb_device_handle = c_void_p +class _libusb_transfer(Structure): + pass +_libusb_transfer_p = POINTER(_libusb_transfer) + +_libusb_transfer_cb_fn_p = CFUNCTYPE(None, _libusb_transfer_p) + +_libusb_transfer._fields_ = [('dev_handle', _libusb_device_handle), + ('flags', c_uint8), + ('endpoint', c_uint8), + ('type', c_uint8), + ('timeout', c_uint), + ('status', c_int), # enum libusb_transfer_status + ('length', c_int), + ('actual_length', c_int), + ('callback', _libusb_transfer_cb_fn_p), + ('user_data', py_object), + ('buffer', c_void_p), + ('num_iso_packets', c_int), + ('iso_packet_desc', _libusb_iso_packet_descriptor) +] + +def _get_iso_packet_list(transfer): + list_type = _libusb_iso_packet_descriptor * transfer.num_iso_packets + return list_type.from_address(addressof(transfer.iso_packet_desc)) + +_lib = None + def _load_library(): if sys.platform != 'cygwin': candidates = ('usb-1.0', 'libusb-1.0', 'usb') for candidate in candidates: + if sys.platform == 'win32': + candidate = candidate + '.dll' + libname = ctypes.util.find_library(candidate) if libname is not None: break else: @@ -247,7 +335,7 @@ def _setup_prototypes(lib): lib.libusb_set_configuration.argtypes = [_libusb_device_handle, c_int] # int libusb_get_configuration(libusb_device_handle *dev, - # int *config) + # int *config) lib.libusb_get_configuration.argtypes = [_libusb_device_handle, POINTER(c_int)] # int libusb_claim_interface(libusb_device_handle *dev, @@ -340,7 +428,7 @@ def _setup_prototypes(lib): lib.libusb_control_transfer.argtypes = [ _libusb_device_handle, c_uint8, - c_uint8, + c_uint8, c_uint16, c_uint16, POINTER(c_ubyte), @@ -382,6 +470,79 @@ def _setup_prototypes(lib): c_uint ] + # libusb_transfer* libusb_alloc_transfer(int iso_packets); + lib.libusb_alloc_transfer.argtypes = [c_int] + lib.libusb_alloc_transfer.restype = POINTER(_libusb_transfer) + + # void libusb_free_transfer(struct libusb_transfer *transfer) + lib.libusb_free_transfer.argtypes = [POINTER(_libusb_transfer)] + + # int libusb_submit_transfer(struct libusb_transfer *transfer); + lib.libusb_submit_transfer.argtypes = [POINTER(_libusb_transfer)] + + # void libusb_set_iso_packet_lengths( + # libusb_transfer* transfer, + # unsigned int length + # ); + def libusb_set_iso_packet_lengths(transfer_p, length): + r"""This function is inline in the libusb.h file, so we must implement + it. + + lib.libusb_set_iso_packet_lengths.argtypes = [ + POINTER(_libusb_transfer), + c_int + ] + """ + transfer = transfer_p.contents + for iso_packet_desc in _get_iso_packet_list(transfer): + iso_packet_desc.length = length + lib.libusb_set_iso_packet_lengths = libusb_set_iso_packet_lengths + + #int libusb_get_max_iso_packet_size(libusb_device* dev, + # unsigned char endpoint); + lib.libusb_get_max_iso_packet_size.argtypes = [c_void_p, + c_ubyte] + + # void libusb_fill_iso_transfer( + # struct libusb_transfer* transfer, + # libusb_device_handle* dev_handle, + # unsigned char endpoint, + # unsigned char* buffer, + # int length, + # int num_iso_packets, + # libusb_transfer_cb_fn callback, + # void * user_data, + # unsigned int timeout + # ); + def libusb_fill_iso_transfer(_libusb_transfer_p, dev_handle, endpoint, buffer, length, + num_iso_packets, callback, user_data, timeout): + r"""This function is inline in the libusb.h file, so we must implement + it. + + lib.libusb_fill_iso_transfer.argtypes = [ + _libusb_transfer, + _libusb_device_handle, + c_ubyte, + POINTER(c_ubyte), + c_int, + c_int, + _libusb_transfer_cb_fn_p, + c_void_p, + c_uint + ] + """ + transfer = _libusb_transfer_p.contents + transfer.dev_handle = dev_handle + transfer.endpoint = endpoint + transfer.type = _LIBUSB_TRANSFER_TYPE_ISOCHRONOUS + transfer.timeout = timeout + transfer.buffer = cast(buffer, c_void_p) + transfer.length = length + transfer.num_iso_packets = num_iso_packets + transfer.user_data = user_data + transfer.callback = callback + lib.libusb_fill_iso_transfer = libusb_fill_iso_transfer + # uint8_t libusb_get_bus_number(libusb_device *dev) lib.libusb_get_bus_number.argtypes = [c_void_p] lib.libusb_get_bus_number.restype = c_uint8 @@ -389,6 +550,7 @@ def _setup_prototypes(lib): # uint8_t libusb_get_device_address(libusb_device *dev) lib.libusb_get_device_address.argtypes = [c_void_p] lib.libusb_get_device_address.restype = c_uint8 + try: # uint8_t libusb_get_port_number(libusb_device *dev) lib.libusb_get_port_number.argtypes = [c_void_p] @@ -396,13 +558,15 @@ def _setup_prototypes(lib): except AttributeError: pass + #int libusb_handle_events(libusb_context *ctx); + lib.libusb_handle_events.argtypes = [c_void_p] + # check a libusb function call def _check(retval): if isinstance(retval, int): retval = c_int(retval) if isinstance(retval, c_int): if retval.value < 0: - from usb.core import USBError ret = retval.value raise USBError(_str_error[ret], ret, _libusb_errno[ret]) return retval @@ -432,20 +596,13 @@ class _ConfigDescriptor(object): def __getattr__(self, name): return getattr(self.desc.contents, name) -# initialize and finalize the library -class _Initializer(object): - def __init__(self): - _check(_lib.libusb_init(None)) - def __del__(self): - _lib.libusb_exit(None) - # iterator for libusb devices class _DevIterator(object): - def __init__(self): + def __init__(self, ctx): self.dev_list = POINTER(c_void_p)() self.num_devs = _check(_lib.libusb_get_device_list( - None, + ctx, byref(self.dev_list)) ).value def __iter__(self): @@ -454,30 +611,104 @@ class _DevIterator(object): def __del__(self): _lib.libusb_free_device_list(self.dev_list, 1) +class _DeviceHandle(object): + def __init__(self, dev): + self.handle = _libusb_device_handle() + self.devid = dev.devid + _check(_lib.libusb_open(self.devid, byref(self.handle))) + +class _IsoTransferHandler(object): + def __init__(self, dev_handle, ep, buff, timeout): + address, length = buff.buffer_info() + + packet_length = _lib.libusb_get_max_iso_packet_size(dev_handle.devid, ep) + packet_count = int(math.ceil(float(length) / packet_length)) + + self.transfer = _lib.libusb_alloc_transfer(packet_count) + + _lib.libusb_fill_iso_transfer(self.transfer, + dev_handle.handle, + ep, + cast(address, POINTER(c_ubyte)), + length, + packet_count, + _libusb_transfer_cb_fn_p(self.__callback), + None, + timeout) + + self.__set_packets_length(length, packet_length) + + def __del__(self): + _lib.libusb_free_transfer(self.transfer) + + def submit(self, ctx = None): + self.__callback_done = 0 + _check(_lib.libusb_submit_transfer(self.transfer)) + + while not self.__callback_done: + _check(_lib.libusb_handle_events(ctx)) + + return self.__compute_size_transf_data() + + def __compute_size_transf_data(self): + return sum([t.actual_length for t in + _get_iso_packet_list(self.transfer.contents)]) + + def __set_packets_length(self, n, packet_length): + _lib.libusb_set_iso_packet_lengths(self.transfer, packet_length) + r = n % packet_length + if r: + iso_packets = _get_iso_packet_list(self.transfer.contents) + iso_packets[-1].length = r + + def __callback(self, transfer): + if transfer.contents.status == LIBUSB_TRANSFER_COMPLETED: + self.__callback_done = 1 + else: + status = int(transfer.contents.status) + raise usb.USBError(_str_transfer_error[status], + status, + _transfer_errno[status]) + # implementation of libusb 1.0 backend class _LibUSB(usb.backend.IBackend): @methodtrace(_logger) + def __init__(self, lib): + usb.backend.IBackend.__init__(self) + self.lib = lib + self.ctx = c_void_p() + _check(self.lib.libusb_init(byref(self.ctx))) + + @methodtrace(_logger) + def __del__(self): + self.lib.libusb_exit(self.ctx) + + + @methodtrace(_logger) def enumerate_devices(self): - return _DevIterator() + return _DevIterator(self.ctx) @methodtrace(_logger) def get_device_descriptor(self, dev): dev_desc = _libusb_device_descriptor() - _check(_lib.libusb_get_device_descriptor(dev.devid, byref(dev_desc))) - dev_desc.bus = _lib.libusb_get_bus_number(dev.devid) - dev_desc.address = _lib.libusb_get_device_address(dev.devid) + _check(self.lib.libusb_get_device_descriptor(dev.devid, byref(dev_desc))) + dev_desc.bus = self.lib.libusb_get_bus_number(dev.devid) + dev_desc.address = self.lib.libusb_get_device_address(dev.devid) + #Only available i newer versions of libusb try: - dev_desc.port_number = _lib.libusb_get_port_number(dev.devid) + dev_desc.port_number = self.lib.libusb_get_port_number(dev.devid) except AttributeError: dev_desc.port_number = None + return dev_desc @methodtrace(_logger) def get_configuration_descriptor(self, dev, config): cfg = POINTER(_libusb_config_descriptor)() - _check(_lib.libusb_get_config_descriptor(dev.devid, - config, byref(cfg))) + _check(self.lib.libusb_get_config_descriptor( + dev.devid, + config, byref(cfg))) return _ConfigDescriptor(cfg) @methodtrace(_logger) @@ -499,41 +730,40 @@ class _LibUSB(usb.backend.IBackend): @methodtrace(_logger) def open_device(self, dev): - handle = _libusb_device_handle() - _check(_lib.libusb_open(dev.devid, byref(handle))) - return handle + return _DeviceHandle(dev) @methodtrace(_logger) def close_device(self, dev_handle): - _lib.libusb_close(dev_handle) + self.lib.libusb_close(dev_handle.handle) @methodtrace(_logger) def set_configuration(self, dev_handle, config_value): - _check(_lib.libusb_set_configuration(dev_handle, config_value)) + _check(self.lib.libusb_set_configuration(dev_handle.handle, config_value)) @methodtrace(_logger) def get_configuration(self, dev_handle): config = c_int() - _check(_lib.libusb_get_configuration(dev_handle, byref(config))) + _check(self.lib.libusb_get_configuration(dev_handle.handle, byref(config))) return config.value @methodtrace(_logger) def set_interface_altsetting(self, dev_handle, intf, altsetting): - _check(_lib.libusb_set_interface_alt_setting(dev_handle, - intf, - altsetting)) + _check(self.lib.libusb_set_interface_alt_setting( + dev_handle.handle, + intf, + altsetting)) @methodtrace(_logger) def claim_interface(self, dev_handle, intf): - _check(_lib.libusb_claim_interface(dev_handle, intf)) + _check(self.lib.libusb_claim_interface(dev_handle.handle, intf)) @methodtrace(_logger) def release_interface(self, dev_handle, intf): - _check(_lib.libusb_release_interface(dev_handle, intf)) + _check(self.lib.libusb_release_interface(dev_handle.handle, intf)) @methodtrace(_logger) def bulk_write(self, dev_handle, ep, intf, data, timeout): - return self.__write(_lib.libusb_bulk_transfer, + return self.__write(self.lib.libusb_bulk_transfer, dev_handle, ep, intf, @@ -542,7 +772,7 @@ class _LibUSB(usb.backend.IBackend): @methodtrace(_logger) def bulk_read(self, dev_handle, ep, intf, size, timeout): - return self.__read(_lib.libusb_bulk_transfer, + return self.__read(self.lib.libusb_bulk_transfer, dev_handle, ep, intf, @@ -551,7 +781,7 @@ class _LibUSB(usb.backend.IBackend): @methodtrace(_logger) def intr_write(self, dev_handle, ep, intf, data, timeout): - return self.__write(_lib.libusb_interrupt_transfer, + return self.__write(self.lib.libusb_interrupt_transfer, dev_handle, ep, intf, @@ -560,22 +790,23 @@ class _LibUSB(usb.backend.IBackend): @methodtrace(_logger) def intr_read(self, dev_handle, ep, intf, size, timeout): - return self.__read(_lib.libusb_interrupt_transfer, + return self.__read(self.lib.libusb_interrupt_transfer, dev_handle, ep, intf, size, timeout) -# TODO: implement isochronous -# @methodtrace(_logger) -# def iso_write(self, dev_handle, ep, intf, data, timeout): -# pass - + @methodtrace(_logger) + def iso_write(self, dev_handle, ep, intf, data, timeout): + handler = _IsoTransferHandler(dev_handle, ep, data, timeout) + return handler.submit(self.ctx) -# @methodtrace(_logger) -# def iso_read(self, dev_handle, ep, intf, size, timeout): -# pass + @methodtrace(_logger) + def iso_read(self, dev_handle, ep, intf, size, timeout): + data = _interop.as_array('\x00' * size) + handler = _IsoTransferHandler(dev_handle, ep, data, timeout) + return data[:handler.submit(self.ctx)] @methodtrace(_logger) def ctrl_transfer(self, @@ -594,15 +825,15 @@ class _LibUSB(usb.backend.IBackend): addr, length = buff.buffer_info() length *= buff.itemsize - ret = _check(_lib.libusb_control_transfer(dev_handle, - bmRequestType, - bRequest, - wValue, - wIndex, - cast(addr, - POINTER(c_ubyte)), - length, - timeout)) + ret = _check(self.lib.libusb_control_transfer( + dev_handle.handle, + bmRequestType, + bRequest, + wValue, + wIndex, + cast(addr, POINTER(c_ubyte)), + length, + timeout)) if usb.util.ctrl_direction(bmRequestType) == usb.util.CTRL_OUT: return ret.value @@ -611,25 +842,26 @@ class _LibUSB(usb.backend.IBackend): @methodtrace(_logger) def reset_device(self, dev_handle): - _check(_lib.libusb_reset_device(dev_handle)) + _check(self.lib.libusb_reset_device(dev_handle.handle)) @methodtrace(_logger) def is_kernel_driver_active(self, dev_handle, intf): - return bool(_check(_lib.libusb_kernel_driver_active(dev_handle, intf))) + return bool(_check(self.lib.libusb_kernel_driver_active(dev_handle.handle, + intf))) @methodtrace(_logger) def detach_kernel_driver(self, dev_handle, intf): - _check(_lib.libusb_detach_kernel_driver(dev_handle, intf)) + _check(self.lib.libusb_detach_kernel_driver(dev_handle.handle, intf)) @methodtrace(_logger) def attach_kernel_driver(self, dev_handle, intf): - _check(_lib.libusb_attach_kernel_driver(dev_handle, intf)) + _check(self.lib.libusb_attach_kernel_driver(dev_handle.handle, intf)) def __write(self, fn, dev_handle, ep, intf, data, timeout): address, length = data.buffer_info() length *= data.itemsize transferred = c_int() - retval = fn(dev_handle, + retval = fn(dev_handle.handle, ep, cast(address, POINTER(c_ubyte)), length, @@ -642,11 +874,11 @@ class _LibUSB(usb.backend.IBackend): return transferred.value def __read(self, fn, dev_handle, ep, intf, size, timeout): - data = _interop.as_array((0,) * size) + data = _interop.as_array('\x00' * size) address, length = data.buffer_info() length *= data.itemsize transferred = c_int() - retval = fn(dev_handle, + retval = fn(dev_handle.handle, ep, cast(address, POINTER(c_ubyte)), length, @@ -658,13 +890,12 @@ class _LibUSB(usb.backend.IBackend): return data[:transferred.value] def get_backend(): - global _lib, _init + global _lib try: if _lib is None: _lib = _load_library() _setup_prototypes(_lib) - _init = _Initializer() - return _LibUSB() + return _LibUSB(_lib) except Exception: _logger.error('Error loading libusb 1.0 backend', exc_info=True) return None diff --git a/pybot/usb/backend/openusb.py b/pybot/usb/backend/openusb.py index 474eecb..3398c4a 100644 --- a/pybot/usb/backend/openusb.py +++ b/pybot/usb/backend/openusb.py @@ -1,8 +1,8 @@ -# Copyright (C) 2009-2011 Wander Lairson Costa -# +# Copyright (C) 2009-2013 Wander Lairson Costa +# # The following terms apply to all files associated # with the software unless explicitly disclaimed in individual files. -# +# # The authors hereby grant permission to use, copy, modify, distribute, # and license this software and its documentation for any purpose, provided # that existing copyright notices are retained in all copies and that this @@ -12,13 +12,13 @@ # and need not follow the licensing terms described here, provided that # the new terms are clearly indicated on the first page of each file where # they apply. -# +# # IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY # FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES # ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY # DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -# +# # THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES, # INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT. THIS SOFTWARE @@ -32,6 +32,8 @@ import usb.util from usb._debug import methodtrace import logging import errno +import sys +import usb._interop as _interop __author__ = 'Wander Lairson Costa' @@ -187,7 +189,7 @@ class _usb_device_desc(Structure): class _openusb_request_result(Structure): _fields_ = [('status', c_int32), - ('transfered_bytes', c_uint32)] + ('transferred_bytes', c_uint32)] class _openusb_ctrl_request(Structure): class _openusb_ctrl_setup(Structure): @@ -195,7 +197,8 @@ class _openusb_ctrl_request(Structure): ('bRequest', c_uint8), ('wValue', c_uint16), ('wIndex', c_uint16)] - _fields_ = [('payload', POINTER(c_uint8)), + _fields_ = [('setup', _openusb_ctrl_setup), + ('payload', POINTER(c_uint8)), ('length', c_uint32), ('timeout', c_uint32), ('flags', c_uint32), @@ -243,7 +246,12 @@ _lib = None _ctx = None def _load_library(): - libname = ctypes.util.find_library('openusb') + candidate = 'openusb' + # Workaround for CPython 3.3 issue#16283 / pyusb #14 + if sys.platform == 'win32': + candidate = candidate + '.dll' + + libname = ctypes.util.find_library(candidate) if libname is None: raise OSError('USB library could not be found') return CDLL(libname) @@ -396,7 +404,7 @@ def _setup_prototypes(lib): ] lib.openusb_parse_interface_desc.restype = c_int32 - + # int32_t openusb_parse_endpoint_desc(openusb_handle_t handle, # openusb_devid_t devid, # uint8_t *buffer, @@ -477,7 +485,10 @@ def _setup_prototypes(lib): lib.openusb_isoc_xfer.restype = c_int32 def _check(retval): - ret = retval.value + if isinstance(retval, c_int): + ret = retval.value + else: + ret = retval if ret != 0: from usb.core import USBError raise USBError(_lib.openusb_strerror(ret), ret, _openusb_errno[ret]) @@ -492,7 +503,7 @@ class _Context(object): class _BusIterator(object): def __init__(self): - self.buslist = POINTER(openusb_busid)() + self.buslist = POINTER(_openusb_busid)() num_busids = c_uint32() _check(_lib.openusb_get_busid_list(_ctx.handle, byref(self.buslist), @@ -599,7 +610,7 @@ class _OpenUSB(usb.backend.IBackend): @methodtrace(_logger) def set_interface_altsetting(self, dev_handle, intf, altsetting): - _check(_lib.set_altsetting(dev_handle, intf, altsetting)) + _check(_lib.openusb_set_altsetting(dev_handle, intf, altsetting)) @methodtrace(_logger) def claim_interface(self, dev_handle, intf): @@ -613,20 +624,22 @@ class _OpenUSB(usb.backend.IBackend): def bulk_write(self, dev_handle, ep, intf, data, timeout): request = _openusb_bulk_request() memset(byref(request), 0, sizeof(request)) - request.payload, request.length = data.buffer_info() + payload, request.length = data.buffer_info() + request.payload = cast(payload, POINTER(c_uint8)) request.timeout = timeout _check(_lib.openusb_bulk_xfer(dev_handle, intf, ep, byref(request))) - return request.transfered_bytes.value + return request.result.transferred_bytes @methodtrace(_logger) def bulk_read(self, dev_handle, ep, intf, size, timeout): request = _openusb_bulk_request() - buffer = array.array('B', '\x00' * size) + buffer = _interop.as_array('\x00' * size) memset(byref(request), 0, sizeof(request)) - request.payload, request.length = buffer.buffer_info() + payload, request.length = buffer.buffer_info() + request.payload = cast(payload, POINTER(c_uint8)) request.timeout = timeout _check(_lib.openusb_bulk_xfer(dev_handle, intf, ep, byref(request))) - return buffer[:request.transfered_bytes.value] + return buffer[:request.result.transferred_bytes] @methodtrace(_logger) def intr_write(self, dev_handle, ep, intf, data, timeout): @@ -636,18 +649,18 @@ class _OpenUSB(usb.backend.IBackend): request.payload = cast(payload, POINTER(c_uint8)) request.timeout = timeout _check(_lib.openusb_intr_xfer(dev_handle, intf, ep, byref(request))) - return request.transfered_bytes.value + return request.result.transferred_bytes @methodtrace(_logger) def intr_read(self, dev_handle, ep, intf, size, timeout): request = _openusb_intr_request() - buffer = array.array('B', '\x00' * size) + buffer = _interop.as_array('B', '\x00' * size) memset(byref(request), 0, sizeof(request)) payload, request.length = buffer.buffer_info() request.payload = cast(payload, POINTER(c_uint8)) request.timeout = timeout _check(_lib.openusb_intr_xfer(dev_handle, intf, ep, byref(request))) - return buffer[:request.transfered_bytes.value] + return buffer[:request.result.transferred_bytes] # TODO: implement isochronous # @methodtrace(_logger) @@ -676,20 +689,20 @@ class _OpenUSB(usb.backend.IBackend): direction = usb.util.ctrl_direction(bmRequestType) - if direction == ENDPOINT_OUT: + if direction == usb.util.CTRL_OUT: buffer = data_or_wLength else: - buffer = array.array('B', '\x00' * data_or_wLength) + buffer = _interop.as_array('\x00' * data_or_wLength) payload, request.length = buffer.buffer_info() request.payload = cast(payload, POINTER(c_uint8)) - ret = _check(_lib.openusb_ctrl_xfer(dev_handle, 0, 0, byref(request))) + _check(_lib.openusb_ctrl_xfer(dev_handle, 0, 0, byref(request))) - if direction == ENDPOINT_OUT: - ret + if direction == usb.util.CTRL_OUT: + return request.result.transferred_bytes else: - buffer[:ret] + return buffer[:request.result.transferred_bytes] @methodtrace(_logger) def reset_device(self, dev_handle): diff --git a/pybot/usb/control.py b/pybot/usb/control.py index 8647c14..c7726da 100644 --- a/pybot/usb/control.py +++ b/pybot/usb/control.py @@ -1,4 +1,4 @@ -# Copyright (C) 2009-2011 Wander Lairson Costa +# Copyright (C) 2009-2013 Wander Lairson Costa # # The following terms apply to all files associated # with the software unless explicitly disclaimed in individual files. diff --git a/pybot/usb/core.py b/pybot/usb/core.py index 597637f..6f79cc8 100644 --- a/pybot/usb/core.py +++ b/pybot/usb/core.py @@ -1,4 +1,4 @@ -# Copyright (C) 2009-2011 Wander Lairson Costa +# Copyright (C) 2009-2013 Wander Lairson Costa # # The following terms apply to all files associated # with the software unless explicitly disclaimed in individual files. diff --git a/pybot/usb/legacy.py b/pybot/usb/legacy.py index 9a9fb95..ebe9f4d 100644 --- a/pybot/usb/legacy.py +++ b/pybot/usb/legacy.py @@ -1,4 +1,4 @@ -# Copyright (C) 2009-2011 Wander Lairson Costa +# Copyright (C) 2009-2013 Wander Lairson Costa # # The following terms apply to all files associated # with the software unless explicitly disclaimed in individual files. @@ -333,12 +333,14 @@ class Device(object): class Bus(object): r"""Bus object.""" - def __init__(self): + def __init__(self, devices): self.dirname = '' self.location = 0 - self.devices = [Device(d) for d in core.find(find_all=True)] + self.devices = [Device(d) for d in devices] def busses(): r"""Return a tuple with the usb busses.""" - return (Bus(),) + return (Bus(g) for k, g in _interop._groupby( + _interop._sorted(core.find(find_all=True), key=lambda d: d.bus), + lambda d: d.bus)) diff --git a/pybot/usb/util.py b/pybot/usb/util.py index da4cb0e..53bcae3 100644 --- a/pybot/usb/util.py +++ b/pybot/usb/util.py @@ -1,4 +1,4 @@ -# Copyright (C) 2009-2011 Wander Lairson Costa +# Copyright (C) 2009-2013 Wander Lairson Costa # # The following terms apply to all files associated # with the software unless explicitly disclaimed in individual files. diff --git a/pybot/usb4butia.py b/pybot/usb4butia.py index eb964d7..2e2e60a 100755..100644 --- a/pybot/usb4butia.py +++ b/pybot/usb4butia.py @@ -3,6 +3,7 @@ # # USB4Butia main # +# Copyright (c) 2012-2013 Alan Aguiar alanjas@hotmail.com # Copyright (c) 2012-2013 Butiá Team butia@fing.edu.uy # Butia is a free and open robotic platform # www.fing.edu.uy/inco/proyectos/butia @@ -24,97 +25,76 @@ import os import imp +import inspect import com_usb from baseboard import Baseboard from device import Device +from functions import ButiaFunctions ERROR = -1 -class USB4Butia(): +class USB4Butia(ButiaFunctions): def __init__(self, debug=False, get_modules=True): - self._debug = debug + self._debug_flag = debug self._hotplug = [] self._openables = [] self._drivers_loaded = {} self._bb = [] - self._modules = [] + self._b_ports = [] self._get_all_drivers() - self.find_butias(get_modules) + self.refresh() + if get_modules: + self.getModulesList(refresh=False) + + def _debug(self, message, err=''): + if self._debug_flag: + print message, err - def get_butia_count(self): + def getButiaCount(self): """ Gets the number of boards detected """ return len(self._bb) - def find_butias(self, get_modules=True): - """ - Search for connected USB4Butia boards and open it - """ - devices = com_usb.find() - for dev in devices: - b = Baseboard(dev) - try: - b.open_baseboard() - self._bb.append(b) - except: - if self._debug: - print 'error open baseboard' - if get_modules: - self.get_modules_list() - - def get_modules_list(self, normal=True): + def getModulesList(self, refresh=True): """ Get the list of modules loaded in the board """ - self._modules = [] - n_boards = self.get_butia_count() - - if self._debug: - print '=Listing Devices' - + self._debug('=Listing Devices') + modules = [] + if refresh: + self.refresh() + n_boards = self.getButiaCount() for i, b in enumerate(self._bb): try: listi = b.get_listi() s = b.get_handler_size() - - if self._debug: - print '===board', i - + self._debug('===board', i) for m in range(0, s + 1): - module_name = listi[b.get_handler_type(m)] - if n_boards > 1: - complete_name = module_name + '@' + str(i) + ':' + str(m) + t = b.get_handler_type(m) + if (t == 255): + self._debug('unknow module in port:%s' % m) else: - complete_name = module_name + ':' + str(m) - - if self._debug: - print '=====module', module_name, (8 - len(module_name)) * ' ', complete_name - - if not(module_name == 'port'): - - if normal: - self._modules.append(complete_name) + module_name = listi[t] + if n_boards > 1: + complete_name = module_name + '@' + str(i) + ':' + str(m) else: - self._modules.append((str(m), module_name, str(i))) - - if not(b.devices.has_key(m) and (b.devices[m].name == module_name)): - d = Device(b, module_name, m) - d.add_functions(self._drivers_loaded[module_name]) - b.add_device(m, d) - - if module_name in self._openables: - b.add_openable_loaded(module_name) - else: - if b.devices.has_key(m): - b.devices.pop(m) - + complete_name = module_name + ':' + str(m) + self._debug('=====module ' + module_name + (9 - len(module_name)) * ' ' + complete_name) + if not(module_name == 'port'): + modules.append(complete_name) + if not(b.devices.has_key(m) and (b.devices[m].name == module_name)): + d = Device(b, module_name, m, self._drivers_loaded[module_name]) + b.add_device(m, d) + if module_name in self._openables: + b.add_openable_loaded(module_name) + else: + if b.devices.has_key(m): + b.devices.pop(m) except Exception, err: - if self._debug: - print 'error module list', err - - return self._modules + self._debug('ERROR:usb4butia:get_modules_list', err) + return modules def _get_all_drivers(self): """ @@ -122,8 +102,7 @@ class USB4Butia(): """ # current folder path_drivers = os.path.join(os.path.dirname(__file__), 'drivers') - if self._debug: - print 'Searching drivers in: ', path_drivers + self._debug('Searching drivers in: ', str(path_drivers)) # normal drivers tmp = os.listdir(path_drivers) tmp.sort() @@ -146,230 +125,194 @@ class USB4Butia(): """ Get a specify driver """ - if self._debug: - print 'Loading driver %s...' % driver + self._debug('Loading driver %s...' % driver) abs_path = os.path.abspath(os.path.join(path, driver + '.py')) - f = None try: - f = imp.load_source(driver, abs_path) + self._drivers_loaded[driver] = imp.load_source(driver, abs_path) except: - if self._debug: - print 'Cannot load %s' % driver, abs_path - if f and hasattr(f, 'FUNCTIONS'): - self._drivers_loaded[driver] = f.FUNCTIONS - else: - if self._debug: - print 'Driver not have FUNCTIONS' - - def callModule(self, modulename, board_number, number, function, params = []): + self._debug('ERROR:usb4butia:_get_driver cannot load %s' % driver, abs_path) + + def callModule(self, modulename, board_number, number, function, params = [], ret_type = int): """ Call one function: function for module: modulename in board: board_name with handler: number (only if the module is pnp, else, the parameter is None) with parameteres: params """ try: + number = int(number) + board_number = int(board_number) + if len(self._bb) < (board_number + 1): + return ERROR board = self._bb[board_number] if board.devices.has_key(number) and (board.devices[number].name == modulename): return board.devices[number].call_function(function, params) else: - if modulename in self._openables: - if modulename in board.get_openables_loaded(): - number = board.get_device_handler(modulename) - else: - board.add_openable_loaded(modulename) - dev = Device(board, modulename) - number = dev.module_open() - dev.add_functions(self._drivers_loaded[modulename]) - board.add_device(number, dev) - return board.devices[number].call_function(function, params) - else: - if self._debug: - print 'no open and no openable' + number = self._open_or_validate(modulename, board) + if number == ERROR: return ERROR + return board.devices[number].call_function(function, params) except Exception, err: - if self._debug: - print 'error call module', err + if hasattr(err, 'errno'): + if (err.errno == 5) or (err.errno == 19): + self.closeB(board) + self._debug('ERROR:usb4butia:callModule', err) return ERROR - def reconnect(self): - """ - Not implemented - """ - pass - def refresh(self): """ - Refresh: if no boards presents, search for them.. else, check if - the boards continues present + Search for connected USB4Butia boards and open it """ - if self._bb == []: - self.find_butias(False) - else: - for b in self._bb: - info = ERROR - try: - info = b.get_info() - except: - if self._debug: - print 'error refresh getinfo' - - if info == ERROR: - self._bb.remove(b) + devices_ports = [] + devices = com_usb.find() + for dev in devices: + n = dev.get_address() + if not(n == None): + devices_ports.append(n) + if not(n in self._b_ports): + b = Baseboard(dev) try: - b.close_baseboard() - except: - pass + b.open_baseboard() + self._bb.append(b) + self._b_ports.append(n) + except Exception, err: + self._debug('ERROR:usb4butia:refresh', err) + + for b in self._bb: + n = b.dev.get_address() + if not(n in devices_ports): + self.closeB(b) + + def closeB(self, b): + try: + n = b.dev.get_address() + self._bb.remove(b) + b.close_baseboard() + if n in self._b_ports: + self._b_ports.remove(n) + except: + pass def close(self): """ Closes all open baseboards """ for b in self._bb: - try: - b.close_baseboard() - except: - if self._debug: - print 'error in close baseboard' + self.closeB(b) self._bb = [] + self._b_ports = [] - def isPresent(self, module_name): + def moduleOpen(self, mod): """ - Check if module: module_name is present - """ - module_list = self.get_modules_list() - return (module_name in module_list) - - def loopBack(self, data, board=0): - """ - LoopBack command: send data to the board and get the result. If all is ok - the return must be exactly of the data parameter - """ - return self.callModule('lback', board, 0, 'send', [data]) - - ################################ Movement calls ################################ - - def set2MotorSpeed(self, leftSense = 0, leftSpeed = 0, rightSense = 0, rightSpeed = 0, board = 0): + Open the module mod """ - Set the speed of 2 motors. The sense is 0 or 1, and the speed is - between 0 and 1023 - """ - msg = [int(leftSense), int(leftSpeed / 256.0), leftSpeed % 256, int(rightSense), int(rightSpeed / 256.0) , rightSpeed % 256] - return self.callModule('motors', board, 0, 'setvel2mtr', msg) - - def setMotorSpeed(self, idMotor = 0, sense = 0, speed = 0, board = 0): - """ - Set the speed of one motor. idMotor = 0 for left motor and 1 for the - right motor. The sense is 0 or 1, and the speed is between 0 and 1023 - """ - msg = [idMotor, sense, int(speed / 256.0), speed % 256] - return self.callModule('motors', board, 0, 'setvelmtr', msg) - - ############################### General calls ############################### - - def getBatteryCharge(self, board=0): - """ - Gets the battery level charge - """ - return self.callModule('butia', board, 0, 'get_volt') - - def getVersion(self, board=0): - """ - Gets the version of Butiá module. 22 for new version - """ - return self.callModule('butia', board, 0, 'read_ver') - - def getFirmwareVersion(self, board=0): - """ - Gets the version of the Firmware - """ - return self.callModule('admin', board, 0, 'getVersion') - - ############################### Sensors calls ############################### + split = self._split_module(mod) + modulename = split[1] + b = int(split[2]) + if len(self._bb) < (b + 1): + return ERROR + board = self._bb[b] + return self._open_or_validate(modulename, board) - def getButton(self, number, board=0): - """ - Gets the value of the button connected in port: number + def _open_or_validate(self, modulename, board): """ - res = self.callModule('button', board, number, 'getValue') - if res != ERROR: - return (1 - res) - else: - return res - - def getLight(self, number, board=0): - """ - Gets the value of the light sensor connected in port: number + Open o check if modulename module is open in board: board """ - m = 65535 - res = self.callModule('light', board, number, 'getValue') - if res != ERROR: - return (m - res) + if modulename in self._openables: + if modulename in board.get_openables_loaded(): + return board.get_device_handler(modulename) + else: + dev = Device(board, modulename, func=self._drivers_loaded[modulename]) + number = dev.module_open() + if number == 255: + self._debug('cannot open module', modulename) + return ERROR + else: + board.add_openable_loaded(modulename) + board.add_device(number, dev) + return number + return ERROR + + def moduleClose(self, mod): + """ + Close the module mod + """ + split = self._split_module(mod) + modulename = split[1] + if modulename in self._openables: + b = int(split[2]) + if len(self._bb) < (b + 1): + return ERROR + board = self._bb[b] + if modulename in board.get_openables_loaded(): + number = board.get_device_handler(modulename) + try: + res = board.devices[number].moduleClose() + if res == 1: + board.remove_openable_loaded(modulename) + return res + except Exception, err: + self._debug('ERROR:usb4butia:moduleClose', err) + return ERROR + else: + self._debug('cannot close no opened module') + return ERROR else: - return res - - def getDistance(self, number, board=0): - """ - Gets the value of the distance sensor connected in port: number - """ - return self.callModule('distanc', board, number, 'getValue') - - def getGray(self, number, board=0): - """ - Gets the value of the gray sensor connected in port: number - """ - return self.callModule('grey', board, number, 'getValue') - - def getTemperature(self, number, board=0): - """ - Gets the value of the temperature sensor connected in port: number - """ - return self.callModule('temp', board, number, 'getValue') - - def getResistance(self, number, board=0): - """ - Gets the value of the resistance sensor connected in port: number - """ - vcc = 65535 - raw = self.callModule('res', board, number, 'getValue') - if not(raw == ERROR): - return raw * 6800 / (vcc - raw) - return raw - - def getVoltage(self, number, board=0): - """ - Gets the value of the voltage sensor connected in port: number - """ - vcc = 65535 - raw = self.callModule('volt', board, number, 'getValue') - if not(raw == ERROR): - return raw * 5 / vcc - return raw - - def setLed(self, number, on_off, board=0): - """ - Sets on or off the LED connected in port: number (0 is off, 1 is on) - """ - return self.callModule('led', board, number, 'turn', [int(on_off)]) - - ################################ Extras ################################ + self._debug('cannot close no openable module') + return ERROR - def modeHack(self, pin, mode, board = 0): + def getListi(self, board_number=0): """ - Sets the mode of hack pin. If mode 0 = input, mode 1 = output + returns a list of instanciables modules """ - msg = [int(pin), int(mode)] - return self.callModule('hackp', board, 0, 'setMode', msg) + board_number = int(board_number) + if len(self._bb) < (board_number + 1): + return [] + board = self._bb[board_number] + listi = board.get_listi(True) + return listi.values() - def setHack(self, pin, value, board = 0): + def _split_module(self, mbn): """ - Sets the value of hack pin configured as output. Value is 0 or 1 + Split a modulename: module@board:port to (number, modulename, board) """ - msg = [int(pin), int(value)] - return self.callModule('hackp', board, 0, 'write', msg) - - def getHack(self, pin, board = 0): - """ - Gets the value of hack pin configured as input. Returns 0 or 1 - """ - return self.callModule('hackp', board, 0, 'read', [int(pin)]) + board = '0' + number = '0' + if mbn.count('@') > 0: + modulename, bn = mbn.split('@') + if bn.count(':') > 0: + board, number = bn.split(':') + else: + board = bn + else: + if mbn.count(':') > 0: + modulename, number = mbn.split(':') + else: + modulename = mbn + return (number, modulename, board) + + def describe(self, mod): + """ + Describe the functions of a modulename + """ + split = self._split_module(mod) + mod = split[1] + funcs = [] + d = {} + if self._drivers_loaded.has_key(mod): + driver = self._drivers_loaded[mod] + a = dir(driver) + flag = False + for p in a: + if flag: + funcs.append(p) + if p == '__package__': + flag = True + for f in funcs: + h = getattr(driver, f) + i = inspect.getargspec(h) + parameters = i[0] + if 'dev' in parameters: + parameters.remove('dev') + d[f] = parameters + return d -- cgit v0.9.1