def _setup_controllers(self, robot): self.log.debug("Setup controllers") self.left_wheel = MotorController(robot, type='wheel', id='LEFT') self.right_wheel = MotorController(robot, type='wheel', id='RIGHT') self.camera = VisionController(robot) self.grabber = ServoController(robot, type='grabber') self.arm = ServoController(robot, type='arm') self._setup_switch_sources(robot) #self.bump_FrL = RuggeduinoController(robot, type='bump', id='FRONT_L') #self.bump_FrR = RuggeduinoController(robot, type='bump', id='FRONT_R') #self.bump_BkL = RuggeduinoController(robot, type='bump', id='BACK_L') #self.bump_BkR = RuggeduinoController(robot, type='bump', id='BACK_R') self.token_sensL = RuggeduinoController(robot, type='sensor', id='TOKEN_L') self.token_sensR = RuggeduinoController(robot, type='sensor', id='TOKEN_R')
class IOInterface(object): def __init__(self, Robot): self.log = logging.getLogger('Robot.IO') self.log.info("Initializing") # Aliases for English readability self.drive = self.turn = self._move try: self._setup_controllers(Robot) except: self.log.critical("Could not set-up hardware controller") raise try: self.running = True tr = ThreadRegister(self) #self.bump_thread = BumpThread() self._bump_callback = lambda s: None self._marker_callback = lambda m: None #tr.add_thread(self.bump_thread) #self.bump_thread.start() except Exception as e: self.log.critical("Unable to start threads") self.log.exception(e) raise self.log.info("Done") def _setup_controllers(self, robot): self.log.debug("Setup controllers") self.left_wheel = MotorController(robot, type='wheel', id='LEFT') self.right_wheel = MotorController(robot, type='wheel', id='RIGHT') self.camera = VisionController(robot) self.grabber = ServoController(robot, type='grabber') self.arm = ServoController(robot, type='arm') self._setup_switch_sources(robot) #self.bump_FrL = RuggeduinoController(robot, type='bump', id='FRONT_L') #self.bump_FrR = RuggeduinoController(robot, type='bump', id='FRONT_R') #self.bump_BkL = RuggeduinoController(robot, type='bump', id='BACK_L') #self.bump_BkR = RuggeduinoController(robot, type='bump', id='BACK_R') self.token_sensL = RuggeduinoController(robot, type='sensor', id='TOKEN_L') self.token_sensR = RuggeduinoController(robot, type='sensor', id='TOKEN_R') def _setup_switch_sources(self, robot): #for id in ['FRONT_L', 'FRONT_R', 'BACK_L', 'BACK_R']: # RuggeduinoController(robot, type='bump_src', id=id).write(False) RuggeduinoController(robot, type='sensor_src', id='TOKEN_L').write(False) RuggeduinoController(robot, type='sensor_src', id='TOKEN_R').write(False) def _move(self, instruction): if not isinstance(instruction, MoveInstruction): raise TypeError("Invalid movement instruction") self.log.debug("Do movement %s", instruction) instruction.action(self) def goto_marker(self, marker, speed, comparator=None, offset=0): self.log.debug("goto marker %s %.1f", marker, speed) if comparator is None or not hasattr(comparator, '__call__'): comparator = lambda a, b: a.info.code == b.info.code reached = False blind_spot = 0.66 while True: # Alternatively, on each new find, call goto_marker within horiz_dist = Marker(marker).horizontal_dist - offset travel_dist = horiz_dist / 2 # set dist to travel half the measured if marker.rot_y < 0: Dir = Left # negative rot_y indicates a left turn else: Dir = Right # positive rot_y means right turn if not reached: # if reached flag is true then should not rotate degree = marker.rot_y if degree < 1: degree += 0.7 # rotation < 1 is too subtle if degree < 2: degree *= 2 # amplify rotation self.turn(Dir(degree, Speed(speed / 3))) # face marker self.drive(Distance(travel_dist, speed)) # drive half way if reached: # set below self.log.info("Reached marker") break potential_markers = [] # new markers of the same type stored here for new_marker in self.get_markers(): if comparator(new_marker, marker): potential_markers.append(new_marker) # append based on comparator closest = self.get_closest_marker(potential_markers) # closest new if closest is None: # there are no markers found if travel_dist < blind_spot: # we have traveled beyond blind_spot self.log.info("Distance to go in blind_spot (%.4f)", travel_dist) reached = True # break on next loop else: self.log.warning("Lost the marker") break # lost the marker else: self.log.debug("Re-found marker") c = Marker(closest) expected = horiz_dist - min(0.1, travel_dist / 3.0) if c.horizontal_dist >= expected: self.log.warning("Did not travel far enough (%f >= %f) (%f, %f)" % ( c.horizontal_dist, expected, closest.dist, marker.dist)) return 0 marker = closest # the new marker to target if not reached: if marker.info.marker_type in MarkerFilter.include("TOKENS"): reached = self.is_holding_token() self.log.debug("Not reached, holding:%s", reached) if reached: break return reached def face_marker(self, marker, speed): self.log.debug("Going to face marker %s", marker) m_orient = abs(marker.orientation.rot_y) if m_orient < 0: Dir1, Dir2 = Left, Right self.log.debug("Going left then right") else: Dir1, Dir2 = Right, Left self.log.debug("Going right then left") self.turn(Dir1(abs(marker.rot_y), Speed(speed))) self.drive(Distance((0.5 * marker.dist) / abs(math.cos(math.radians(m_orient))), speed)) self.turn(Dir2(3 * m_orient, Speed(speed))) def face_marker_2(self, marker, speed): theta = abs(marker.orientation.rot_y) length = marker.dist if theta < 0: Dir1, Dir2 = Left, Right self.log.debug("Going left then right") else: Dir1, Dir2 = Right, Left self.log.debug("Going right then left") self.turn(Dir1(2 * theta, Speed(speed))) x = (length / 2) * math.cos(math.radians(theta)) self.drive(Distance(x, speed)) angle = 2 * theta self.turn(Dir2(angle, Speed(speed))) self.drive(Distance(x, speed)) def face_marker_3(self, marker, speed): self.log.debug("Going to face marker %s", marker) m_orient = abs(marker.orientation.rot_y) if m_orient < 0: Dir1, Dir2 = Left, Right self.log.debug("Going left then right") else: Dir1, Dir2 = Right, Left self.log.debug("Going right then left") self.turn(Dir1(abs(marker.rot_y), Speed(speed))) self.drive(Distance((0.7 * marker.dist) / abs(math.cos(math.radians(m_orient))), speed)) self.turn(Dir2(2 * m_orient, Speed(speed))) def navigate_to_marker(self, marker, speed, comparator=None): self.log.debug("navigating to marker") self.face_marker_3(marker, speed) # face the marker markers = self.get_markers() # rescan for the marker old_marker = marker for new_marker in markers: if new_marker.info.code == marker.info.code: marker = new_marker self.log.debug("Found marker, continuing") break if marker is old_marker: # the marker got lost self.log.warning("Could not find marker") return False if self.goto_marker(marker, speed, comparator): return True markers = self.get_markers() # rescan for the marker for new_marker in markers: if new_marker.info.code == marker.info.code: marker = new_marker self.log.debug("Found marker again, continuing") break if marker is old_marker: # the marker got lost self.log.warning("Could not find marker") return False self.navigate_to_marker(marker, speed, comparator) def is_holding_token(self): return self.token_sensL.read() or self.token_sensR.read() def move_arm(self, direction, delay=False): if direction not in [UP, DOWN, MIDDLE]: raise TypeError("Invalid arm movement") if direction == UP: angle = self.arm.MIN elif direction == DOWN: angle = self.arm.MAX elif direction == MIDDLE: angle = self.arm.MAX / 2 self.log.debug("Move arm %s (angle = %d)", direction, angle) self.arm.set_angle(angle) if delay: self.wait(0.5) def open_grabber(self, delay=False): self.log.debug("OPEN grabber (angle = %d)", self.grabber.MIN) self.grabber.set_angle(self.grabber.MIN) if delay: self.wait(0.7) def close_grabber(self, delay=False): self.log.debug("CLOSE grabber (angle = %d)", self.grabber.MAX) self.grabber.set_angle(self.grabber.MAX) if delay: self.wait(0.7) def get_markers(self, _filter=None): markers = self.camera.find_markers() self._marker_callback(markers) self.log.debug("get_markers %s", str(self.camera.fmt_markers(markers))) if _filter is not None: markers = filter(lambda m: m.info.marker_type in _filter, markers) return markers def get_closest_marker(self, markers): self.log.debug("Get closest marker to robot") return self.camera.get_closest(markers) def get_closest_marker_rotation(self, markers, degree=0): self.log.debug("Get closest marker from degree (%f)", degree) return self.camera.get_rotation_nearest(markers, degree) def _stop_operation(self, filter=None): self.log.info("Stopping current action") try: self.right_wheel.stop() self.left_wheel.stop() except Exception as e: self.log.exception(e) raise StateInterrupt('stop', 'operation.stop') def wait(self, seconds): self.log.debug("Wait %.4f seconds", seconds) time.sleep(seconds) def set_bump_handler(self, callback): self._bump_callback = callback def set_marker_handler(self, callback): self._marker_callback = callback def bumped(self, sensor): if sensor is self.bump_FrL: self._bump_callback('Front.Left') elif sensor is self.bump_FrR: self._bump_callback('Front.Right') elif sensor is self.bump_BkL: self._bump_callback('Back.Left') elif sensor is self.bump_BkR: self._bump_callback('Back.Right') def create_token_obj(self, markers): return Token(markers) def stop(self): self.log.info("Stopping all communications") self.running = False
class IOInterface(object): def __init__(self, Robot): self.log = logging.getLogger('Robot.IO') self.log.info("Initializing") # Aliases for English readability self.drive = self.turn = self._move try: self._setup_controllers(Robot) except: self.log.critical("Could not set-up hardware controller") raise try: self.running = True tr = ThreadRegister(self) #self.bump_thread = BumpThread() self._bump_callback = lambda s: None self._marker_callback = lambda m: None #tr.add_thread(self.bump_thread) #self.bump_thread.start() except Exception as e: self.log.critical("Unable to start threads") self.log.exception(e) raise self.log.info("Done") def _setup_controllers(self, robot): self.log.debug("Setup controllers") self.left_wheel = MotorController(robot, type='wheel', id='LEFT') self.right_wheel = MotorController(robot, type='wheel', id='RIGHT') self.camera = VisionController(robot) self.grabber = ServoController(robot, type='grabber') self.arm = ServoController(robot, type='arm') self._setup_switch_sources(robot) #self.bump_FrL = RuggeduinoController(robot, type='bump', id='FRONT_L') #self.bump_FrR = RuggeduinoController(robot, type='bump', id='FRONT_R') #self.bump_BkL = RuggeduinoController(robot, type='bump', id='BACK_L') #self.bump_BkR = RuggeduinoController(robot, type='bump', id='BACK_R') self.token_sensL = RuggeduinoController(robot, type='sensor', id='TOKEN_L') self.token_sensR = RuggeduinoController(robot, type='sensor', id='TOKEN_R') def _setup_switch_sources(self, robot): #for id in ['FRONT_L', 'FRONT_R', 'BACK_L', 'BACK_R']: # RuggeduinoController(robot, type='bump_src', id=id).write(False) RuggeduinoController(robot, type='sensor_src', id='TOKEN_L').write(False) RuggeduinoController(robot, type='sensor_src', id='TOKEN_R').write(False) def _move(self, instruction): if not isinstance(instruction, MoveInstruction): raise TypeError("Invalid movement instruction") self.log.debug("Do movement %s", instruction) instruction.action(self) def goto_marker(self, marker, speed, comparator=None, offset=0): self.log.debug("goto marker %s %.1f", marker, speed) if comparator is None or not hasattr(comparator, '__call__'): comparator = lambda a, b: a.info.code == b.info.code reached = False blind_spot = 0.66 while True: # Alternatively, on each new find, call goto_marker within horiz_dist = Marker(marker).horizontal_dist - offset travel_dist = horiz_dist / 2 # set dist to travel half the measured if marker.rot_y < 0: Dir = Left # negative rot_y indicates a left turn else: Dir = Right # positive rot_y means right turn if not reached: # if reached flag is true then should not rotate degree = marker.rot_y if degree < 1: degree += 0.7 # rotation < 1 is too subtle if degree < 2: degree *= 2 # amplify rotation self.turn(Dir(degree, Speed(speed / 3))) # face marker self.drive(Distance(travel_dist, speed)) # drive half way if reached: # set below self.log.info("Reached marker") break potential_markers = [] # new markers of the same type stored here for new_marker in self.get_markers(): if comparator(new_marker, marker): potential_markers.append( new_marker) # append based on comparator closest = self.get_closest_marker(potential_markers) # closest new if closest is None: # there are no markers found if travel_dist < blind_spot: # we have traveled beyond blind_spot self.log.info("Distance to go in blind_spot (%.4f)", travel_dist) reached = True # break on next loop else: self.log.warning("Lost the marker") break # lost the marker else: self.log.debug("Re-found marker") c = Marker(closest) expected = horiz_dist - min(0.1, travel_dist / 3.0) if c.horizontal_dist >= expected: self.log.warning( "Did not travel far enough (%f >= %f) (%f, %f)" % (c.horizontal_dist, expected, closest.dist, marker.dist)) return 0 marker = closest # the new marker to target if not reached: if marker.info.marker_type in MarkerFilter.include("TOKENS"): reached = self.is_holding_token() self.log.debug("Not reached, holding:%s", reached) if reached: break return reached def face_marker(self, marker, speed): self.log.debug("Going to face marker %s", marker) m_orient = abs(marker.orientation.rot_y) if m_orient < 0: Dir1, Dir2 = Left, Right self.log.debug("Going left then right") else: Dir1, Dir2 = Right, Left self.log.debug("Going right then left") self.turn(Dir1(abs(marker.rot_y), Speed(speed))) self.drive( Distance( (0.5 * marker.dist) / abs(math.cos(math.radians(m_orient))), speed)) self.turn(Dir2(3 * m_orient, Speed(speed))) def face_marker_2(self, marker, speed): theta = abs(marker.orientation.rot_y) length = marker.dist if theta < 0: Dir1, Dir2 = Left, Right self.log.debug("Going left then right") else: Dir1, Dir2 = Right, Left self.log.debug("Going right then left") self.turn(Dir1(2 * theta, Speed(speed))) x = (length / 2) * math.cos(math.radians(theta)) self.drive(Distance(x, speed)) angle = 2 * theta self.turn(Dir2(angle, Speed(speed))) self.drive(Distance(x, speed)) def face_marker_3(self, marker, speed): self.log.debug("Going to face marker %s", marker) m_orient = abs(marker.orientation.rot_y) if m_orient < 0: Dir1, Dir2 = Left, Right self.log.debug("Going left then right") else: Dir1, Dir2 = Right, Left self.log.debug("Going right then left") self.turn(Dir1(abs(marker.rot_y), Speed(speed))) self.drive( Distance( (0.7 * marker.dist) / abs(math.cos(math.radians(m_orient))), speed)) self.turn(Dir2(2 * m_orient, Speed(speed))) def navigate_to_marker(self, marker, speed, comparator=None): self.log.debug("navigating to marker") self.face_marker_3(marker, speed) # face the marker markers = self.get_markers() # rescan for the marker old_marker = marker for new_marker in markers: if new_marker.info.code == marker.info.code: marker = new_marker self.log.debug("Found marker, continuing") break if marker is old_marker: # the marker got lost self.log.warning("Could not find marker") return False if self.goto_marker(marker, speed, comparator): return True markers = self.get_markers() # rescan for the marker for new_marker in markers: if new_marker.info.code == marker.info.code: marker = new_marker self.log.debug("Found marker again, continuing") break if marker is old_marker: # the marker got lost self.log.warning("Could not find marker") return False self.navigate_to_marker(marker, speed, comparator) def is_holding_token(self): return self.token_sensL.read() or self.token_sensR.read() def move_arm(self, direction, delay=False): if direction not in [UP, DOWN, MIDDLE]: raise TypeError("Invalid arm movement") if direction == UP: angle = self.arm.MIN elif direction == DOWN: angle = self.arm.MAX elif direction == MIDDLE: angle = self.arm.MAX / 2 self.log.debug("Move arm %s (angle = %d)", direction, angle) self.arm.set_angle(angle) if delay: self.wait(0.5) def open_grabber(self, delay=False): self.log.debug("OPEN grabber (angle = %d)", self.grabber.MIN) self.grabber.set_angle(self.grabber.MIN) if delay: self.wait(0.7) def close_grabber(self, delay=False): self.log.debug("CLOSE grabber (angle = %d)", self.grabber.MAX) self.grabber.set_angle(self.grabber.MAX) if delay: self.wait(0.7) def get_markers(self, _filter=None): markers = self.camera.find_markers() self._marker_callback(markers) self.log.debug("get_markers %s", str(self.camera.fmt_markers(markers))) if _filter is not None: markers = filter(lambda m: m.info.marker_type in _filter, markers) return markers def get_closest_marker(self, markers): self.log.debug("Get closest marker to robot") return self.camera.get_closest(markers) def get_closest_marker_rotation(self, markers, degree=0): self.log.debug("Get closest marker from degree (%f)", degree) return self.camera.get_rotation_nearest(markers, degree) def _stop_operation(self, filter=None): self.log.info("Stopping current action") try: self.right_wheel.stop() self.left_wheel.stop() except Exception as e: self.log.exception(e) raise StateInterrupt('stop', 'operation.stop') def wait(self, seconds): self.log.debug("Wait %.4f seconds", seconds) time.sleep(seconds) def set_bump_handler(self, callback): self._bump_callback = callback def set_marker_handler(self, callback): self._marker_callback = callback def bumped(self, sensor): if sensor is self.bump_FrL: self._bump_callback('Front.Left') elif sensor is self.bump_FrR: self._bump_callback('Front.Right') elif sensor is self.bump_BkL: self._bump_callback('Back.Left') elif sensor is self.bump_BkR: self._bump_callback('Back.Right') def create_token_obj(self, markers): return Token(markers) def stop(self): self.log.info("Stopping all communications") self.running = False