def sweep():
cube = turn_to_block()
if cube != None:
drivers.move(drivers.RobotState(drivers.DRIVE, -cube.dist / 3))
cube = turn_to_block()
if cube != None:
drivers.move(drivers.RobotState(drivers.DRIVE, -cube.dist / 5))
drivers.move(drivers.RobotState(drivers.TURN, math.pi))
drivers.move(drivers.RobotState(drivers.DRIVE, 10))
collecting = True
return cube
def turn_to_block():
src = camera.capture()
drivers.LED3.on()
objects, mask, cvxhull = mod.process(src)
drivers.LED3.off()
draw(src, objects)
cv2.imwrite("out.jpg", src)
cubes = find_blocks(objects)
best_cube = None
for cube in cubes:
area = cube.rect[2] * cube.rect[3]
print("area", area)
horizon_req = cube.rect[1] > 220
dist_req = cube.dist > 0
area_req = area > 500
print(horizon_req, dist_req, area_req)
if horizon_req and dist_req and area_req and (
best_cube == None
or area > best_cube.rect[2] * best_cube.rect[3]):
best_cube = cube
print("new best has area", area)
# Trying for a cube
if best_cube != None:
midpoint = best_cube.rect[0] + best_cube.rect[2] / 2
print("mid", midpoint)
arc = (midpoint - 320) / 320 * CAMERA_FOV / 2
print("arc", arc)
drivers.move(drivers.RobotState(drivers.TURN, -arc))
return best_cube
# System initialization
drivers.init()
camera = Camera()
mod = VisionModule(width=640, height=480)
# State
pose = ROBOT_ORIGIN_POSES[identity]
time = 0
iterations = 0
done = False
epoch = time.time()
# Parker robots have a simple routine
if identity in PARKER_ROBOTS:
drivers.move(drivers.RobotState(drivers.DRIVE, in_to_cm(ROBOT_AXIAL + 2)))
sleep(3)
drivers.move(drivers.RobotState(drivers.DRIVE, -in_to_cm(ROBOT_AXIAL + 3)))
done = True
# Collector robots will wait a bit for parkers to park
elif identity in COLLECTOR_ROBOTS:
sleep(8)
goal_direction = math.atan2(goal[1] - pose[1], goal[0] - pose[0])
while not done:
# Run vision on current FOV, set indicators
src = camera.capture()
drivers.LED3.on()
objects, mask, cvxhull = mod.process(src)
drivers.LED3.off()
identity = None
with open("identity.dat", "r") as file:
identity = file.readlines()[0].strip()
print(identity,
"online. Pray to Lafayette Official God.\nWaiting for start signal")
while not os.path.isfile("go"):
pass
os.remove("go")
print("Go time!")
camera = Camera()
mod = VisionModule(width=640, height=480)
collecting = False
drivers.move(drivers.RobotState(drivers.DRIVE, -in_to_cm(6)))
def sweep():
cube = turn_to_block()
if cube != None:
drivers.move(drivers.RobotState(drivers.DRIVE, -cube.dist / 3))
cube = turn_to_block()
if cube != None:
drivers.move(drivers.RobotState(drivers.DRIVE, -cube.dist / 5))
drivers.move(drivers.RobotState(drivers.TURN, math.pi))
drivers.move(drivers.RobotState(drivers.DRIVE, 10))
collecting = True
return cube
print("Initializing drivers...")
drivers.init()
# Identify which robot I am
identity = None
with open("identity.dat", "r") as file:
identity = file.readlines()[0].strip()
print(identity, "spinning up. Pray to Lafayette Official God.")
# Wait for server to distribute start signal
print("Waiting for start signal.")
while not os.path.isfile(START_SIGNAL):
pass
if identity == PARKER_NAME:
print("Parking...")
# Back in and out
drivers.move(
drivers.RobotState(drivers.DRIVE, in_to_cm(ROBOT_AXIAL_LENGTH + 2)))
time.sleep(2)
drivers.move(
drivers.RobotState(drivers.DRIVE, -in_to_cm(ROBOT_AXIAL_LENGTH + 2)))
# Signal done
drivers.LED1.on()
os.remove(START_SIGNAL)
print("Done.")
time.sleep(10)