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 main(): s_controller = ServoController() smart_lock = SmartLock() functions = {} functions["cw"] = s_controller.rotate_clockwise functions["ccw"] = s_controller.rotate_counterclockwise functions["ct"] = s_controller.centerlize option = get_options("--operation") if option == "lock": while not smart_lock.is_locked(): functions["ccw"]() print("locked") return elif option == "unlock": while smart_lock.is_locked(): functions["cw"]() print("unlocked") return while True: print("時計回:cw, 反時計回:ccw, 静止位置キャリブレーション:ct") cmd = input() try: functions[cmd]() print("is_locked: %s" % smart_lock.is_locked()) except: del s_controller break
def serv1(port): try: from servo_controller import ServoController servo = ServoController() except Exception as e: log.debug("Failed to use ServoController: {0}".format(e)) from standins import StandinServoController servo = StandinServoController() try: from motor_controller import MotorController motors = MotorController() except Exception as e: log.debug("failed to use MotorController: {0}".format(e)) from standins import StandinMotorController, StandinMotorDefinition motors = StandinMotorController() motors.defineMotor("right", (14, 15), servo, 0) motors.defineMotor("left", (17, 18), servo, 1) motors.setSignedPWM("left", 0) motors.setSignedPWM("right", 0) time.sleep(0.1) motors.setDirection("left", "B") time.sleep(0.1) motors.setDirection("left", "A") time.sleep(0.1) motors.setDirection("right", "B") time.sleep(0.1) motors.setDirection("right", "A") time.sleep(0.1) addr = ('', port) server = HTTPServer(addr, PyServ) server.servo = servo server.motors = motors server.autodriver = None os.chdir("content") log.debug("Serving on port {0} in {1}".format(port, os.getcwd())) server.serve_forever()
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
def dispense(): ServoController().dispense() return 'Yiiiisss I very good doggo!'
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
port=config.APA102_PORT, device=config.APA102_DEVICE, bus_speed_hz=config.APA102_BUS_SPEED_HZ) apa102.set_contrast(config.APA102_DEFAULT_CONTRAST) apa102_controller = APA102Controller(apa102=apa102) servo = Servo( servo_gpio=config.SERVO_GPIO, pulse_left_ns=config.SERVO_PULSE_LEFT_NS, pulse_right_ns=config.SERVO_PULSE_RIGHT_NS) servo_controller = ServoController(servo) mqtt_listener = MQTTListener(config) mqtt_listener.connect() if __name__ == '__main__': try: pause() except KeyboardInterrupt: apa102.clear() servo.idle() mqtt_listener.disconnect() print("Bye")