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 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
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()
