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 _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 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")
