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