Пример #1
0
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()
Пример #2
0
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()
Пример #3
0
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)
Пример #4
0
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)
Пример #5
0
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()
Пример #6
0
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()