diff options
Diffstat (limited to 'pybot/usb4butia.py')
| -rwxr-xr-x | pybot/usb4butia.py | 375 |
1 files changed, 375 insertions, 0 deletions
diff --git a/pybot/usb4butia.py b/pybot/usb4butia.py new file mode 100755 index 0000000..eb964d7 --- /dev/null +++ b/pybot/usb4butia.py @@ -0,0 +1,375 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- +# +# USB4Butia main +# +# 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 com_usb +from baseboard import Baseboard +from device import Device + +ERROR = -1 + +class USB4Butia(): + + def __init__(self, debug=False, get_modules=True): + self._debug = debug + self._hotplug = [] + self._openables = [] + self._drivers_loaded = {} + self._bb = [] + self._modules = [] + self._get_all_drivers() + self.find_butias(get_modules) + + def get_butia_count(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): + """ + Get the list of modules loaded in the board + """ + self._modules = [] + n_boards = self.get_butia_count() + + if self._debug: + print '=Listing Devices' + + for i, b in enumerate(self._bb): + try: + listi = b.get_listi() + s = b.get_handler_size() + + if self._debug: + print '===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) + 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) + 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) + + except Exception, err: + if self._debug: + print 'error module list', err + + return self._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') + if self._debug: + print 'Searching drivers in: ', 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 + """ + if self._debug: + print '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) + 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 = []): + """ + 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: + 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' + return ERROR + except Exception, err: + if self._debug: + print 'error call module', 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 + """ + 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) + try: + b.close_baseboard() + 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._bb = [] + + 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 + """ + return self.callModule('lback', board, 0, 'send', [data]) + + ################################ 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 = [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 ############################### + + def getButton(self, number, board=0): + """ + Gets the value of the button connected in port: number + """ + 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 + """ + m = 65535 + res = self.callModule('light', board, number, 'getValue') + if res != ERROR: + return (m - res) + 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 ################################ + + def modeHack(self, pin, mode, board = 0): + """ + Sets the mode of hack pin. If mode 0 = input, mode 1 = output + """ + msg = [int(pin), int(mode)] + return self.callModule('hackp', 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 = [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)]) + |