def __init__(self, device='/dev/ttyUSB0', debug=False):
'''Inicializa el dispositivo de conexion con el/los robot/s'''
self.board = DuinoBot(device, debug=debug)
self._generic_initialization()
class Board(object):
MOTOR_DELAY_TB6612 = timedelta(0, 0, 100000)
lock = threading.Lock()
def __ignore_motors(self, robot_id, left=None, right=None):
'''Para evitar dañar las placas los cambios de velocidad,
principalmente si invierten el sentido de giro de los motores,
deben limitarse a 100ms por recomendación del fabricante.'''
now = datetime.now()
old_time = self._last_move.get(
robot_id, now - timedelta(0, 1, 0)
)
if left == 0 and right == 0:
# Siempre dejar pasar los stop() sin registrarlos
return False
if now - old_time <= self.MOTOR_DELAY_TB6612:
return True
self._last_move[robot_id] = now
return False
def __init__(self, device='/dev/ttyUSB0', debug=False):
'''Inicializa el dispositivo de conexion con el/los robot/s'''
self.board = DuinoBot(device, debug=debug)
self._generic_initialization()
def _generic_initialization(self):
it = util.Iterator(self.board)
# FIXME: En Python > 2.5 se puede cambiar por it.daemon = True
# http://stackoverflow.com/questions/17650754
# /how-can-i-capture-ctrl-d-in-python-interactive-console
it.setDaemon(True)
it.start()
self.board.pass_time(0.1)
self._last_move = dict()
def __del__(self):
self.exit()
def __getattribute__(self, name):
'''Permitir multithreading para evitar problemas con el método
senses'''
Board.lock.acquire()
try:
res = object.__getattribute__(self, name)
except:
Board.lock.release()
raise
Board.lock.release()
return res
def motor0(self, vel, robotid):
if vel >= 0 and vel <= 100\
and not self.__ignore_motors(robotid, vel):
self.board.send_sysex(1, [int(vel), robotid])
def motor1(self, vel, robotid):
if vel >= 0 and vel <= 100\
and not self.__ignore_motors(robotid, right=vel):
self.board.send_sysex(2, [int(vel), robotid])
def motors(self, vel1, vel2, seconds=-1, robotid=0):
if abs(vel1) <= 100 and abs(vel2) <= 100\
and not self.__ignore_motors(robotid, vel1, vel2):
self.board.send_sysex(
4,
[int(abs(vel1)), int(abs(vel2)), 1
if vel1 > 0 else 0, 1 if vel2 > 0 else 0, robotid])
if seconds != -1:
self.board.pass_time(seconds)
self.motors(0, 0, -1, robotid)
def ping(self, robotid):
self.board.nearest_obstacle[robotid] = None
self.board.send_sysex(3, [robotid])
# A veces hay que esperar más de 0.04 segundos
for i in xrange(6):
# wait 20ms (ping delay) + 20ms (comm)
self.board.pass_time(0.04)
if self.board.nearest_obstacle[robotid] is not None:
return self.board.nearest_obstacle[robotid]
return -1
def sleep(self, time):
self.board.pass_time(time)
def exit(self):
try:
self.board.exit()
except AttributeError:
# Si falla antes de inicializar self.board, entonces no hace falta
# hacer nada.
pass
def analog(self, ch, samples=1, robotid=0):
# PyFirmata espera que el canal de los analógicos sea un número del 0
# al 6 (si bien del lado del robot este número se convierte de nuevo a
# un número de pin sumandole el valor de la macro A0).
ch = ch - A0
self.board.send_sysex(PIN_COMMANDS, [PIN_GET_ANALOG, ch, samples, robotid])
self.board.pass_time(0.04)
return self.board.pin_analog_value(robotid)[ch]
def battery(self, robotid):
return self.analog(A6, 1, robotid) * 5.0 / 1024
def digital(self, pin, robotid=0):
self.board.send_sysex(PIN_COMMANDS, [PIN_GET_DIGITAL, pin, robotid])
self.board.pass_time(0.04)
return self.board.pin_digital_value(robotid)[pin]
def set_pin_mode(self, pin, mode, robotid):
self.board.send_sysex(EXTENDED_PIN_MODE, [pin, mode, robotid])
def beep(self, freq=0, microseconds=0, robotid=0):
hi = freq >> 7
lo = freq % 128
if freq != 0:
if microseconds == 0:
self.board.send_sysex(5, [hi, lo, robotid])
else:
self.board.send_sysex(
5, [hi, lo, int(microseconds*1000), robotid])
# self.board.pass_time(microseconds)
else:
self.board.send_sysex(5, [robotid])
def report(self):
self.board.live_robots = [0 for i in range(128)]
self.board.send_sysex(9, [])
self.board.pass_time(0.5)
robotlist = []
for i in range(128):
if self.board.live_robots[i]:
robotlist.append(i)
return robotlist
def set_id(self, new_id, robotid):
self.board.send_sysex(8, [new_id, robotid])
self.board.pass_time(0.02)
def getLine(self, robotid):
a = self.analog(A4, robotid=robotid)
b = self.analog(A5, robotid=robotid)
return (a, b)
def getWheels(self, robotid):
a = self.analog(A1, robotid=robotid)
b = self.analog(A2, robotid=robotid)
return (a, b)
wait = sleep
