class PlayGame:
def __init__(self, log, io, corner):
self.log = log
self.io = io
self.our_tokens = game_map.get_our_tokens(corner)
self.log.info("Token codes for this match: %s", str(self.our_tokens))
self.corner = corner
self.startup()
def AdjDir(self, Dir):
if self.corner in [0, 2]:
return Dir
self.log.info("Adjusted direction (from %s)", str(Dir))
return Left if Dir is Right else Right
def startup(self):
self.state = StateMachine({"START": self.preset_start,
"SEARCH_SLOT": self.search_slot,
"DRIVE_TO_SLOT": self.drive_to_slot,
"PLACE_TOKEN": self.place_token,
"TURN_TO_TOKEN": self.next_token,
"SEARCH_TOKEN": self.search_token,
"DRIVE_TO_TOKEN": self.drive_to_token,
"PICK_TOKEN": self.pickup_token,
"TURN_TO_SLOT": self.next_slot})
self.state.bind('change', self.state_changed)
self.state.bind('error', self.state_error)
self.state.bind('finish', self.state_finished)
self.state.bind('interrupt', self.state_interrupted)
self.on_interrupt = None
self.reset()
for state in self.state.next_state():
self.state.change_state(state)
def set_state(self, state, *args):
self.state.set_state(state, args)
def state_changed(self, fromstate, tostate):
self.log.info("State Changed from %s to %s", fromstate, tostate)
def state_error(self, state, errors):
self.log.error(str(state) + " encountered errors")
for e in errors:
self.log.exception(e)
def state_finished(self, state):
self.log.info(str(state) + " finished")
def state_interrupted(self, state, ex):
self.log.info(str(state) + " interrupted" + str(ex))
if self.on_interrupt:
self.on_interrupt()
self.on_interrupt = None
def reset(self):
self.io.set_bump_handler(self.onbump)
self.io.set_marker_handler(self.handle_markers)
self.io.open_grabber()
self.io.move_arm(DOWN, True)
self.has_token = False
self.searches = {'SLOTS': 0, 'TOKENS': 0}
self.from_start = False
self.done_count = 0
self.set_state("START")
def onbump(self, bump):
self.log.info(bump)
def handle_markers(self, markers):
currstate = self.state.get_active_state().name
obsticles = MarkerFilter.include('ROBOTS', 'WALLS')
for marker in markers:
if marker.info.marker_type in obsticles:
if marker.dist < 0.5:
def after_interrupt():
self.log.info("Move away")
self.io.drive(Distance(-0.9, 50))
self.io.turn(Right(10, Speed(50)))
self.on_interrupt = after_interrupt
self.log.info("Obsticle found, %s", str(marker))
self.io._stop_operation()
if marker.info.marker_type in MarkerFilter.include('SLOTS'):
if currstate == "SEARCH_TOKEN" and marker.rot_y < 5:
self.log.info("Found slot while searching for token")
def itrpt():
self.io.turn(Right(90, Speed(50)))
self.set_state("SEARCH_TOKEN")
self.on_interrupt = itrpt()
raise StateInterrupt('slotfound', [])
if marker.info.marker_type in MarkerFilter.include("WALLS"):
if currstate == "SEARCH_TOKEN":
self.log.info("Check walls here")
# check looking at correct wall
continue
#game_map.get_arena_codes_for_corner(self.corner)
def preset_start(self):
self.from_start = True
self.log.info("Grabbing token")
self.io.close_grabber(True)
self.io.move_arm(UP, True)
self.has_token = True
self.log.info("Drive 2.3 meters")
self.io.drive(Distance(2.3, 80))
self.io.move_arm(MIDDLE, True)
self.io.turn(self.AdjDir(Left)(40, Speed(60)))
self.set_state("SEARCH_SLOT")
def search_slot(self):
def filt(markers):
if not self.from_start: return markers
for marker in markers:
if marker.info.code == game_map.closest_slot(self.corner):
return [marker]
return markers
self.search_func("SLOTS", self.AdjDir(Right), "DRIVE_TO_SLOT", max_rot=270,
rot_cb=lambda:self.io.drive(Distance(0.5, 50)), filt=filt)
def search_func(self, type, Dir, next_state, max_rot=200, rot_cb=None,
filt=None):
if not hasattr(rot_cb, '__call__'):
rot_cb = lambda: self.io.drive(Distance(1, 50))
if not hasattr(filt, '__call__'):
filt = lambda m:m
found = False
self.searches[type] += 1
rotation = 0
self.log.info("Looking for %s", type)
while not found:
markers = self.io.get_markers(MarkerFilter.include(type))
markers = filt(markers)
if len(markers) > 0:
marker = self.io.get_closest_marker(markers)
self.log.info("Found marker, %s", str(marker))
found = True
elif rotation > max_rot:
self.log.info("Turned too much")
found = rot_cb()
rotation = 0
else:
rotation += self.scan_move(Dir)
self.set_state(next_state, marker)
def drive_to_slot(self, slot):
assert self.has_token
if self.io.goto_marker(slot, 70, offset=0.3): #navigate_to_marker
self.log.info("Got to slot")
self.set_state("PLACE_TOKEN")
elif self.from_start:
self.log.info("From start, just place down")
self.io.drive(Distance(0.1, 40))
self.set_state("PLACE_TOKEN")
elif self.searches["SLOTS"] >= 2:
self.log.info("Searched for >= 2 times, drop here")
self.set_state("PLACE_TOKEN")
else:
self.log.info("Search again")
self.set_state("SEARCH_SLOT")
def place_token(self):
assert self.has_token
self.from_start = False
self.searches["SLOTS"] = 0
self.io.move_arm(UP, True)
self.io.open_grabber()
self.io.move_arm(DOWN)
self.has_token = False
self.done_count += 1
self.set_state("TURN_TO_TOKEN")
def next_token(self):
self.log.info("Head to next token")
self.io.drive(Distance(-0.7, 60)) # reverse out of zone
if self.done_count < 2:
self.log.info("Turn face near token")
self.io.turn(self.AdjDir(Left)(147, Speed(50)))
self.io.drive(Distance(1, 65))
elif self.done_count == 2:
self.log.info("Face far token")
self.io.turn(self.AdjDir(Right)(94, Speed(50)))
self.io.drive(Distance(2, 65))
self.set_state("SEARCH_TOKEN")
def search_token(self):
def check(tokens):
return filter(lambda m:m.info.code in self.our_tokens, tokens)
self.search_func("TOKENS", self.AdjDir(Left), "DRIVE_TO_TOKEN", filt=check)
def drive_to_token(self, token):
assert not self.has_token
result = self.io.goto_marker(token, 70) #navigate_to_marker
if result:
self.set_state("PICK_TOKEN")
else:
self.log.info("lost marker (searching again)")
if result is 0:
self.log.info("Got stuck, reversing")
self.io.drive(Distance(-0.7, 50))
self.io.turn(self.AdjDir(Right)(90, Speed(50), WHEEL))
self.set_state("SEARCH_TOKEN")
def pickup_token(self):
if not self.io.is_holding_token():
self.log.info("Not holding token, move 0.3m")
self.io.drive(Distance(0.3, 30))
else:
self.log.info("Holding token")
self.io.close_grabber(True)
self.log.info("Pull token out")
self.io.move_arm(MIDDLE, True)
self.io.drive(Distance(-0.2, 50))
self.io.move_arm(UP, True)
self.has_token = True
self.set_state("TURN_TO_SLOT")
def next_slot(self):
self.io.drive(Distance(-0.5, 50)) # Carry token out further
if not self.check_has_token():
self.log.warning("Dont have token")
self.io.move_arm(DOWN)
self.io.open_grabber()
self.has_token = False
self.io.drive(Distance(-0.5, 80))
self.set_state("SEARCH_TOKEN")
return
self.log.info("Has token")
if self.done_count < 2:
self.io.turn(self.AdjDir(Left)(133, Speed(50)))
self.io.drive(Distance(2.1, 50))
self.io.turn(self.AdjDir(Left)(46, Speed(50)))
else:
self.io.turn(Left(170, Speed(50)))
self.io.drive(Distance(2, 70))
self.set_state("SEARCH_SLOT")
def check_has_token(self):
tokens = self.io.get_markers([MarkerFilter.include("TOKENS")[1]])
tokens = filter(lambda m:m.info.code in self.our_tokens, tokens)
tokens = filter(lambda m:m.dist < 0.5, tokens)
has_token = len(tokens)
has_token = has_token or self.io.is_holding_token()
return has_token
def scan_move(self, Direction):
degree = 17
self.log.info("Scanning, turning %s by %d", Direction, degree)
self.io.turn(Direction(degree, Speed(50)))
self.io.wait(1)
return degree
class PlayGame:
def __init__(self, robot, corner):
self.log = logging.getLogger('Robot.Logic')
self.robot = robot
self.corner = corner
self.corners = [
range(0, 4) + range(24, 28),
range(3, 11),
range(11, 18),
range(18, 25)
]
self.startup()
def AdjDir(self, Dir):
if self.corner in [0, 2]:
return Dir
self.log.info("Adjusted direction (from %s)", str(Dir))
return Left if Dir is Right else Right
def startup(self):
strategy = StrategyRegistry.get('STRAT_1')
self.sm = StateMachine(strategy.get_states())
self.sm.bus.register('change', self.state_changed)
self.sm.bus.register('error', self.state_error)
self.sm.bus.register('finish', self.state_finished)
self.sm.bus.register('interrupt', self.state_interrupted)
EventBus.GLOBAL.register('markers', self.handle_markers)
EventBus.GLOBAL.register('bump', self.handle_bump)
EventBus.GLOBAL.register('__bus_error__', self.handle_bus_error)
self.sm.bus.register('__bus_error__', self.handle_bus_error)
self.reset()
strategy.set_game_util(self)
for state in self.sm.next_state():
self.sm.change_state(state)
def set_state(self, state, *args):
self.sm.set_state(state, args)
def state_changed(self, payload):
fromstate, tostate = payload
self.log.info("State Changed from %s to %s", fromstate, tostate)
def state_error(self, payload):
state, errors = payload
self.log.error(str(state) + " encountered errors")
for e in errors:
self.log.exception(e['exception'])
def state_finished(self, state):
self.log.info(str(state) + " finished")
def state_interrupted(self, payload):
state, ex = payload
self.log.info("%s was interrupted, %s", state, ex)
def reset(self):
self.sm.set_state("MAIN")
def handle_bus_error(self, payload):
channel, handler, exception = payload
if isinstance(exception, StateInterrupt):
raise exception
self.log.error("Error occured when handling the channel '%s'" \
+ " in the handler %s", channel, handler)
self.log.exception(exception)
def handle_markers(self, markers):
self.log.debug("Handling marker scan")
currstate = self.sm.get_active_state().name
obsticles = markers \
.filter(lambda m: m.info.marker_type in ['robot', 'arena']) \
.filter(lambda m: m.dist < 0.4)
if obsticles.is_empty:
return
nearest_obsticle = obsticles.get_closest()
def on_interrupt(payload):
self.sm.bus.unregister('interrupt', on_interrupt)
self.log.info("Move away")
self.robot.wheels.backward(0.9, 50)
self.robot.wheels.right(10, 50)
self.sm.bus.register('interrupt', on_interrupt)
self.log.info("Obsticle found, %s", nearest_obsticle)
raise StateInterrupt('stop', 'operation.stop')
def handle_bump(self, sensors):
self.log.warn("Bumped on sensors %s", sensors)
self.robot.wheels.backward(0.7, 60)
self.robot.wheels.left(30, 60)
class PlayGame:
def __init__(self, robot, corner):
self.log = logging.getLogger('Robot.Logic')
self.robot = robot
self.corner = corner
self.corners = [
range(0, 4) + range(24, 28),
range(3, 11),
range(11, 18),
range(18, 25)
]
self.startup()
def AdjDir(self, Dir):
if self.corner in [0, 2]:
return Dir
self.log.info("Adjusted direction (from %s)", str(Dir))
return Left if Dir is Right else Right
def startup(self):
strategy = StrategyRegistry.get('STRAT_1')
self.sm = StateMachine(strategy.get_states())
self.sm.bus.register('change', self.state_changed)
self.sm.bus.register('error', self.state_error)
self.sm.bus.register('finish', self.state_finished)
self.sm.bus.register('interrupt', self.state_interrupted)
EventBus.GLOBAL.register('markers', self.handle_markers)
EventBus.GLOBAL.register('bump', self.handle_bump)
EventBus.GLOBAL.register('__bus_error__', self.handle_bus_error)
self.sm.bus.register('__bus_error__', self.handle_bus_error)
self.reset()
strategy.set_game_util(self)
for state in self.sm.next_state():
self.sm.change_state(state)
def set_state(self, state, *args):
self.sm.set_state(state, args)
def state_changed(self, payload):
fromstate, tostate = payload
self.log.info("State Changed from %s to %s", fromstate, tostate)
def state_error(self, payload):
state, errors = payload
self.log.error(str(state) + " encountered errors")
for e in errors:
self.log.exception(e['exception'])
def state_finished(self, state):
self.log.info(str(state) + " finished")
def state_interrupted(self, payload):
state, ex = payload
self.log.info("%s was interrupted, %s", state, ex)
def reset(self):
self.sm.set_state("MAIN")
def handle_bus_error(self, payload):
channel, handler, exception = payload
if isinstance(exception, StateInterrupt):
raise exception
self.log.error("Error occured when handling the channel '%s'" \
+ " in the handler %s", channel, handler)
self.log.exception(exception)
def handle_markers(self, markers):
self.log.debug("Handling marker scan")
currstate = self.sm.get_active_state().name
obsticles = markers \
.filter(lambda m: m.info.marker_type in ['robot', 'arena']) \
.filter(lambda m: m.dist < 0.4)
if obsticles.is_empty:
return
nearest_obsticle = obsticles.get_closest()
def on_interrupt(payload):
self.sm.bus.unregister('interrupt', on_interrupt)
self.log.info("Move away")
self.robot.wheels.backward(0.9, 50)
self.robot.wheels.right(10, 50)
self.sm.bus.register('interrupt', on_interrupt)
self.log.info("Obsticle found, %s", nearest_obsticle)
raise StateInterrupt('stop', 'operation.stop')
def handle_bump(self, sensors):
self.log.warn("Bumped on sensors %s", sensors)
self.robot.wheels.backward(0.7, 60)
self.robot.wheels.left(30, 60)
class PlayGame:
def __init__(self, log, io, corner):
self.log = log
self.io = io
self.our_tokens = game_map.get_our_tokens(corner)
self.log.info("Token codes for this match: %s", str(self.our_tokens))
self.corner = corner
self.startup()
def AdjDir(self, Dir):
if self.corner in [0, 2]:
return Dir
self.log.info("Adjusted direction (from %s)", str(Dir))
return Left if Dir is Right else Right
def startup(self):
self.state = StateMachine({
"START": self.preset_start,
"SEARCH_SLOT": self.search_slot,
"DRIVE_TO_SLOT": self.drive_to_slot,
"PLACE_TOKEN": self.place_token,
"TURN_TO_TOKEN": self.next_token,
"SEARCH_TOKEN": self.search_token,
"DRIVE_TO_TOKEN": self.drive_to_token,
"PICK_TOKEN": self.pickup_token,
"TURN_TO_SLOT": self.next_slot
})
self.state.bind('change', self.state_changed)
self.state.bind('error', self.state_error)
self.state.bind('finish', self.state_finished)
self.state.bind('interrupt', self.state_interrupted)
self.on_interrupt = None
self.reset()
for state in self.state.next_state():
self.state.change_state(state)
def set_state(self, state, *args):
self.state.set_state(state, args)
def state_changed(self, fromstate, tostate):
self.log.info("State Changed from %s to %s", fromstate, tostate)
def state_error(self, state, errors):
self.log.error(str(state) + " encountered errors")
for e in errors:
self.log.exception(e)
def state_finished(self, state):
self.log.info(str(state) + " finished")
def state_interrupted(self, state, ex):
self.log.info(str(state) + " interrupted" + str(ex))
if self.on_interrupt:
self.on_interrupt()
self.on_interrupt = None
def reset(self):
self.io.set_bump_handler(self.onbump)
self.io.set_marker_handler(self.handle_markers)
self.io.open_grabber()
self.io.move_arm(DOWN, True)
self.has_token = False
self.searches = {'SLOTS': 0, 'TOKENS': 0}
self.from_start = False
self.done_count = 0
self.set_state("START")
def onbump(self, bump):
self.log.info(bump)
def handle_markers(self, markers):
currstate = self.state.get_active_state().name
obsticles = MarkerFilter.include('ROBOTS', 'WALLS')
for marker in markers:
if marker.info.marker_type in obsticles:
if marker.dist < 0.5:
def after_interrupt():
self.log.info("Move away")
self.io.drive(Distance(-0.9, 50))
self.io.turn(Right(10, Speed(50)))
self.on_interrupt = after_interrupt
self.log.info("Obsticle found, %s", str(marker))
self.io._stop_operation()
if marker.info.marker_type in MarkerFilter.include('SLOTS'):
if currstate == "SEARCH_TOKEN" and marker.rot_y < 5:
self.log.info("Found slot while searching for token")
def itrpt():
self.io.turn(Right(90, Speed(50)))
self.set_state("SEARCH_TOKEN")
self.on_interrupt = itrpt()
raise StateInterrupt('slotfound', [])
if marker.info.marker_type in MarkerFilter.include("WALLS"):
if currstate == "SEARCH_TOKEN":
self.log.info("Check walls here")
# check looking at correct wall
continue
#game_map.get_arena_codes_for_corner(self.corner)
def preset_start(self):
self.from_start = True
self.log.info("Grabbing token")
self.io.close_grabber(True)
self.io.move_arm(UP, True)
self.has_token = True
self.log.info("Drive 2.3 meters")
self.io.drive(Distance(2.3, 80))
self.io.move_arm(MIDDLE, True)
self.io.turn(self.AdjDir(Left)(40, Speed(60)))
self.set_state("SEARCH_SLOT")
def search_slot(self):
def filt(markers):
if not self.from_start: return markers
for marker in markers:
if marker.info.code == game_map.closest_slot(self.corner):
return [marker]
return markers
self.search_func("SLOTS",
self.AdjDir(Right),
"DRIVE_TO_SLOT",
max_rot=270,
rot_cb=lambda: self.io.drive(Distance(0.5, 50)),
filt=filt)
def search_func(self,
type,
Dir,
next_state,
max_rot=200,
rot_cb=None,
filt=None):
if not hasattr(rot_cb, '__call__'):
rot_cb = lambda: self.io.drive(Distance(1, 50))
if not hasattr(filt, '__call__'):
filt = lambda m: m
found = False
self.searches[type] += 1
rotation = 0
self.log.info("Looking for %s", type)
while not found:
markers = self.io.get_markers(MarkerFilter.include(type))
markers = filt(markers)
if len(markers) > 0:
marker = self.io.get_closest_marker(markers)
self.log.info("Found marker, %s", str(marker))
found = True
elif rotation > max_rot:
self.log.info("Turned too much")
found = rot_cb()
rotation = 0
else:
rotation += self.scan_move(Dir)
self.set_state(next_state, marker)
def drive_to_slot(self, slot):
assert self.has_token
if self.io.goto_marker(slot, 70, offset=0.3): #navigate_to_marker
self.log.info("Got to slot")
self.set_state("PLACE_TOKEN")
elif self.from_start:
self.log.info("From start, just place down")
self.io.drive(Distance(0.1, 40))
self.set_state("PLACE_TOKEN")
elif self.searches["SLOTS"] >= 2:
self.log.info("Searched for >= 2 times, drop here")
self.set_state("PLACE_TOKEN")
else:
self.log.info("Search again")
self.set_state("SEARCH_SLOT")
def place_token(self):
assert self.has_token
self.from_start = False
self.searches["SLOTS"] = 0
self.io.move_arm(UP, True)
self.io.open_grabber()
self.io.move_arm(DOWN)
self.has_token = False
self.done_count += 1
self.set_state("TURN_TO_TOKEN")
def next_token(self):
self.log.info("Head to next token")
self.io.drive(Distance(-0.7, 60)) # reverse out of zone
if self.done_count < 2:
self.log.info("Turn face near token")
self.io.turn(self.AdjDir(Left)(147, Speed(50)))
self.io.drive(Distance(1, 65))
elif self.done_count == 2:
self.log.info("Face far token")
self.io.turn(self.AdjDir(Right)(94, Speed(50)))
self.io.drive(Distance(2, 65))
self.set_state("SEARCH_TOKEN")
def search_token(self):
def check(tokens):
return filter(lambda m: m.info.code in self.our_tokens, tokens)
self.search_func("TOKENS",
self.AdjDir(Left),
"DRIVE_TO_TOKEN",
filt=check)
def drive_to_token(self, token):
assert not self.has_token
result = self.io.goto_marker(token, 70) #navigate_to_marker
if result:
self.set_state("PICK_TOKEN")
else:
self.log.info("lost marker (searching again)")
if result is 0:
self.log.info("Got stuck, reversing")
self.io.drive(Distance(-0.7, 50))
self.io.turn(self.AdjDir(Right)(90, Speed(50), WHEEL))
self.set_state("SEARCH_TOKEN")
def pickup_token(self):
if not self.io.is_holding_token():
self.log.info("Not holding token, move 0.3m")
self.io.drive(Distance(0.3, 30))
else:
self.log.info("Holding token")
self.io.close_grabber(True)
self.log.info("Pull token out")
self.io.move_arm(MIDDLE, True)
self.io.drive(Distance(-0.2, 50))
self.io.move_arm(UP, True)
self.has_token = True
self.set_state("TURN_TO_SLOT")
def next_slot(self):
self.io.drive(Distance(-0.5, 50)) # Carry token out further
if not self.check_has_token():
self.log.warning("Dont have token")
self.io.move_arm(DOWN)
self.io.open_grabber()
self.has_token = False
self.io.drive(Distance(-0.5, 80))
self.set_state("SEARCH_TOKEN")
return
self.log.info("Has token")
if self.done_count < 2:
self.io.turn(self.AdjDir(Left)(133, Speed(50)))
self.io.drive(Distance(2.1, 50))
self.io.turn(self.AdjDir(Left)(46, Speed(50)))
else:
self.io.turn(Left(170, Speed(50)))
self.io.drive(Distance(2, 70))
self.set_state("SEARCH_SLOT")
def check_has_token(self):
tokens = self.io.get_markers([MarkerFilter.include("TOKENS")[1]])
tokens = filter(lambda m: m.info.code in self.our_tokens, tokens)
tokens = filter(lambda m: m.dist < 0.5, tokens)
has_token = len(tokens)
has_token = has_token or self.io.is_holding_token()
return has_token
def scan_move(self, Direction):
degree = 17
self.log.info("Scanning, turning %s by %d", Direction, degree)
self.io.turn(Direction(degree, Speed(50)))
self.io.wait(1)
return degree