def run_avoiding_walls(r, coro):
try:
task = asyncio.ensure_future(coro)
while not task.done():
if r.left_short_ir.val and r.right_short_ir.val and util.close_to_wall(
r):
task.cancel()
log.debug("All sensors hit")
if r.left_bumper.val:
yield From(r.drive.turn_angle(np.radians(-120)))
else:
yield From(r.drive.turn_angle(np.radians(120)))
if r.left_bumper.val or r.left_short_ir.val:
log.debug("Left side triggered")
task.cancel()
yield From(avoid_wall(r, r.left_short_ir, r.left_bumper, -1))
if r.right_bumper.val or r.right_short_ir.val:
log.debug("Right side triggered")
task.cancel()
yield From(avoid_wall(r, r.right_short_ir, r.right_bumper, 1))
yield
try:
task.result()
except asyncio.CancelledError:
pass
finally:
task.cancel()
def run_avoiding_walls(r, coro):
try:
task = asyncio.ensure_future(coro)
while not task.done():
if r.left_short_ir.val and r.right_short_ir.val and util.close_to_wall(r):
task.cancel()
log.debug("All sensors hit")
if r.left_bumper.val:
yield From(r.drive.turn_angle(np.radians(-120)))
else:
yield From(r.drive.turn_angle(np.radians(120)))
if r.left_bumper.val or r.left_short_ir.val:
log.debug("Left side triggered")
task.cancel()
yield From(avoid_wall(r,r.left_short_ir,r.left_bumper,-1))
if r.right_bumper.val or r.right_short_ir.val:
log.debug("Right side triggered")
task.cancel()
yield From(avoid_wall(r,r.right_short_ir,r.right_bumper,1))
yield
try:
task.result()
except asyncio.CancelledError:
pass
finally:
task.cancel()
def clean_up(r):
log.info('Doing round cleanup')
r.drive.stop()
startAngle = r.drive.odometer.val.theta
# Turn until find good direction or if we spin all the way around
try:
task = asyncio.ensure_future(r.drive.turn_speed(np.radians(30)))
while not task.done():
gap_found = False
if abs(r.drive.odometer.val.theta - startAngle) >= 2 * pi:
task.cancel()
if not util.close_to_wall(r):
task.cancel()
gap_found = True
yield
try:
task.result()
except asyncio.CancelledError:
pass
finally:
task.cancel()
if not gap_found:
log.debug('Rotated 2pi, no gaps')
r.drive.stop()
yield asyncio.sleep(0.5)
r.silo.open()
yield asyncio.sleep(1.5)
if gap_found:
log.debug('Going forward to leave stack, because there\'s space')
Drive.go_distance(r.drive, 6)
def clean_up(r):
log.info('Doing round cleanup')
r.drive.stop()
startAngle = r.drive.odometer.val.theta
# Turn until find good direction or if we spin all the way around
try:
task = asyncio.ensure_future(r.drive.turn_speed(np.radians(30)))
while not task.done():
gap_found = False
if abs(r.drive.odometer.val.theta - startAngle) >= 2*pi:
task.cancel()
if not util.close_to_wall(r):
task.cancel()
gap_found = True
yield
try:
task.result()
except asyncio.CancelledError:
pass
finally:
task.cancel()
if not gap_found:
log.debug('Rotated 2pi, no gaps')
r.drive.stop()
yield asyncio.sleep(0.5)
r.silo.open()
yield asyncio.sleep(1.5)
if gap_found:
log.debug('Going forward to leave stack, because there\'s space')
Drive.go_distance(r.drive, 6)
def find_cubes(r):
try:
search_task = None
while True:
if r.break_beams.blocked:
log.info("break beams were broken")
yield From(
run_picking_up_cubes(
r, run_avoiding_walls(r, r.drive.go_distance(6))))
yield From(pick_up_cubes(r))
try:
yield From(asyncio.get_event_loop().run_in_executor(
None, v.update))
except IOError:
continue
m.update_cubes_from(v)
cube = v.nearest_cube()
#print cube
# start scanning for cubes
if cube is None:
if search_task is None:
log.info('No cubes in view - scanning')
search_task = asyncio.ensure_future(search_for_cubes(r))
continue
# we found a cube - stop scanning
if search_task:
# clean up the old task
search_task.cancel()
while not search_task.done():
yield
try:
search_task.result()
except asyncio.CancelledError:
pass
search_task = None
log.info('Stopped scanning')
if abs(cube.angle_to) > np.radians(10):
log.debug("Turning {} to {}".format(cube.angle_to, cube))
yield From(r.drive.turn_angle(cube.angle_to))
else:
log.debug("Going {}in to {}".format(cube.distance, cube))
# limit distance
to_go = cube.pos2
if cube.distance > 60:
to_go = to_go * 60 / cube.distance
# transform to world space
to_go = np.append(to_go, 1)
dest = r.drive.odometer.val.robot_matrix.dot(to_go)
try:
yield From(
run_picking_up_cubes(
r,
run_avoiding_walls(
r, r.drive.go_to(dest[:2],
throw_timeout=True))))
except asyncio.TimeoutError:
log.warn("Driving task timed out and we caught it")
if util.close_to_wall(r):
# TODO: Make smarter thing than just moving arbitrarily
Drive.go_distance(r.drive, -1)
yield From(r.drive.turn_angle(np.radians(45)))
finally:
if search_task:
search_task.cancel()
def find_cubes(r):
try:
search_task = None
while True:
if r.break_beams.blocked:
log.info("break beams were broken")
yield From(run_picking_up_cubes(r, run_avoiding_walls(r, r.drive.go_distance(6))))
yield From(pick_up_cubes(r))
try:
yield From(asyncio.get_event_loop().run_in_executor(None, v.update))
except IOError:
continue
m.update_cubes_from(v)
cube = v.nearest_cube()
#print cube
# start scanning for cubes
if cube is None:
if search_task is None:
log.info('No cubes in view - scanning')
search_task = asyncio.ensure_future(search_for_cubes(r))
continue
# we found a cube - stop scanning
if search_task:
# clean up the old task
search_task.cancel()
while not search_task.done():
yield
try:
search_task.result()
except asyncio.CancelledError:
pass
search_task = None
log.info('Stopped scanning')
if abs(cube.angle_to) > np.radians(10):
log.debug("Turning {} to {}".format(cube.angle_to, cube))
yield From(r.drive.turn_angle(cube.angle_to))
else:
log.debug("Going {}in to {}".format(cube.distance, cube))
# limit distance
to_go = cube.pos2
if cube.distance > 60:
to_go = to_go * 60 / cube.distance
# transform to world space
to_go = np.append(to_go, 1)
dest = r.drive.odometer.val.robot_matrix.dot(to_go)
try:
yield From(
run_picking_up_cubes(r, run_avoiding_walls(r,
r.drive.go_to(dest[:2], throw_timeout=True)
))
)
except asyncio.TimeoutError:
log.warn("Driving task timed out and we caught it")
if util.close_to_wall(r):
# TODO: Make smarter thing than just moving arbitrarily
Drive.go_distance(r.drive, -1)
yield From(r.drive.turn_angle(np.radians(45)))
finally:
if search_task:
search_task.cancel()
