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
from sw.hal import ColorSensor from sw.vision import Colors import tamproxy import pygame import sys import numpy as np import time import itertools import os if __name__ == '__main__': with tamproxy.TAMProxy() as tamp: color = ColorSensor(tamp) screen = pygame.display.set_mode([500, 500]) while True: val = color.val print val screen.fill(Colors.to_rgb(val)) time.sleep(0.1) pygame.display.flip() for event in pygame.event.get(): if event.type == pygame.QUIT: sys.exit()