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)