def main():
R = systemetric.Robot()
m = Map(arena=CompetitionArenaMap())
mr = MapRenderer(m)
mr.startInNewWindow()
while True:
with Timer("Profiling") as t:
with t.event("see"): see = R.see()
with t.event("processed"): processed = see.processed()
with t.event("update"): m.updateEntities(processed)
m.fakeUpdateEntities(
tokens={2: Point2(1, 1), 12: Point2(0.5, 1.75), 0: Point2(3, 1.25)},
transform=Matrix3.new_translate(2, 2.5).rotate(math.pi/3)
)
time.sleep(2)
m.fakeUpdateEntities(
tokens={2: Point2(1.5, 1.75), 13: Point2(0.5, 0.75), 5: Point2(3.5, 2.25)},
transform=Matrix3.new_translate(4, 2.5).rotate(math.pi*2/3)
)
def main():
def grabCube():
R.power.beep(440, 1)
time.sleep(1)
R.power.beep(880, 1)
time.sleep(1)
R.power.beep(440, 1)
time.sleep(1)
def driveTowards(relativeLocation, token):
"""Drive at most 1 meter towards a cube. Return True if the cube was reached"""
R.turnToFace(relativeLocation)
distance = abs(relativeLocation)
ROBOT_SIZE = 0.2
if distance < ROBOT_SIZE:
print "Found %s" % token
return True
elif distance > 1 + ROBOT_SIZE:
print "More than 1m from %s" % token
R.driveDistance(1)
return False
else:
print "Less than 1m from %s" % token
R.driveDistance(distance - ROBOT_SIZE + 0.05)
return False
gameMap = Map(arena=S007ArenaMap())
R = systemetric.Robot()
startTime = time.time()
while True:
print
print time.time() - startTime
vision = R.see().processed()
gameMap.updateEntities(vision)
if gameMap.robot:
print "Robot at", gameMap.robot.location
distanceTo = {}
knownTokens = filter(lambda i: i.exists, gameMap.tokens)
print gameMap.tokens
if knownTokens:
for entity in knownTokens:
distanceTo[entity] = entity.position - gameMap.robot.location
nearestToken = min(distanceTo, key = lambda e: abs(distanceTo[e]))
print "Nearest marker is #%s at %s" % (nearestToken.id, nearestToken.position)
vectorToCube = gameMap.robot.transform.inverse() * distanceTo[nearestToken]
gotCube = driveTowards(vectorToCube, nearestToken)
if gotCube:
grabCube()
nearestToken.invalidate()
else:
gameMap.invalidateRobotPosition()
else:
print "No tokens found"
print "Spin on spot"
#no tokens yet!
R.rotateBy(40)
R.stop()
else:
print "I'm lost!"
print "Spin on spot"
#no tokens yet, and I'm lost!
R.rotateBy(40)
R.stop()
time.sleep(0.25)
