def pick_up_cubes(r): global ganked_cube while True: val = get_cube(r) if val != Colors.NONE: r.drive.stop() yield From(asyncio.sleep(0.1)) val = get_cube(r) if val == OUR_COLOR: ganked_cube = time.time() r.arms.silo.up() log.info('Picked up {} block'.format(Colors.name(val))) r.arms.silo.down() elif val == THEIR_COLOR: ganked_cube = time.time() r.drive.stop() r.arms.dump.up() log.info('Picked up {} block'.format(Colors.name(val))) r.arms.dump.down() else: break else: break yield From(asyncio.sleep(0.05))
def get_cube(r): val = r.color_sensor.val blocked = r.break_beams.blocked if val == Colors.NONE: if USE_BREAKBEAM and blocked: log.warn('Beam broken, but no color reading') return Colors.NONE if USE_BREAKBEAM and not blocked: log.warn('Color is {}, but beam not broken'.format(Colors.name(val))) return Colors.NONE return val