def ballGo(endTime,freq=10.0):
global nBalls
pose = ct.getPose()
balls = getBalls()
knownBalls = getKnownBalls(balls)
if len(knownBalls) > 0:
print "Picking up ball :)"
ct.clearWayPoints()
ct.setWayPointControl()
nBalls = nBalls + len(knownBalls)
ct.addWayPoints([v.getBallCoords(x,pose) + [0,False] for x in knownBalls])
elif len(balls) > 0:
print "Going to ball :P"
# Find largest size ball
b = balls[0]
for bPrime in balls[1:]:
if bPrime.size > b.size:
b = bPrime
goTowardsArea(balls[0])
while (not ct.waitingForCommand()) and (endTime > time.time()):
startTime = time.time()
pose = ct.getPose()
knownBalls = getKnownBalls(getBalls())
if len(knownBalls) > 0:
print "Picking up ball :)"
ct.clearWayPoints()
ct.setWayPointControl()
ct.addWayPoints([v.getBallCoords(x,pose) + [0,False] for x in knownBalls])
return
time.sleep(max(0,1.0/freq - (time.time()-startTime)))
def goTowardsArea(area):
pose = ct.getPose()
theta = v.getAreaAngle(area,pose)
point = getPoint(pose,theta,1000000)
ct.clearWayPoints()
ct.setWayPointControl()
ct.addWayPoints([list(point) + [0,False]])
def pickUpBall(area, freq = 10.0):
coords = v.getBallCoords(area,ct.getPose())
if coords is not None:
ct.clearWayPoints()
ct.setWayPointControl()
ct.addWayPoints([list(coords) + [0,False]])
while not ct.waitingForCommand():
time.sleep(1.0/freq)
