def start():
global myMap,q,mapThread#,pygameThread
q = False
mapThread = threading.Thread(target=goMapping)
#pygameThread = threading.Thread(target=goPygame)
ct.initialize()
v.initialize()
mapThread.start()
#pygameThread.start()
ct.setWallFollowControl()
def main(freq = 100.0):
global nBalls
nBalls = 0
v.initialize()
ct.initialize()
startMapping()
endTime = time.time()+3*60
while endTime > time.time():
startTime = time.time()
if len(getBalls()) > 0:
ballGoAndRotate(endTime)
elif len(getWalls()) > 0 and (nBalls >= 3 or (nBalls >= 1 and endTime-time.time() <= 30)):
scoreGo(endTime)
else:
ct.setWallFollowControl()
time.sleep(max(0,1.0/freq - (time.time()-startTime)))
def goSim(freq=50.0):
global myMap,q,mapThread#,pygameThread
q = False
mapThread = threading.Thread(target=goMapping)
#pygameThread = threading.Thread(target=goPygame)
v.initialize()
ct.initialize()
mapThread.start()
#pygameThread.start()
FOLLOW = 0
GRAB = 1
APPROACH_ORIENT = 2
APPROACH_GO = 3
SPIN = 4
spinDuration = 5.0
spinStart = time.time()
state = FOLLOW
ct.setWallFollowControl()
startTime = time.time()
while not q and ((time.time()-startTime) <= 3*60):
start = time.time()
pose = ct.getPose()
myMap.setConfigSpace()
balls = [x for x in v.getAreas()if v.isBall(x)
and canDrive(pose,getPoint(pose,v.getAreaAngle(x,pose),driveDistance))]
knownBalls = []
for b in balls:
bc = v.getBallCoords(b,pose)
if bc is not None and canDrive(pose, bc):
knownBalls.append(bc)
if state == FOLLOW:
if len(knownBalls) != 0:
state = GRAB
print "Heading to grab ball"
ct.setWayPointControl()
ct.addWayPoints([[a[0],a[1],0,False] for a in knownBalls])
elif len(balls) != 0:
state = APPROACH_ORIENT
print "Going towards ball 0"
ct.setWayPointControl()
ct.addWayPoints([[pose[0],pose[1],v.getAreaAngle(balls[0],pose),True]])
elif state == GRAB:
if ct.waitingForCommand():
if len(knownBalls) != 0:
ct.addWayPoints([[a[0],a[1],0,False] for a in knownBalls])
else:
ct.setBasicControl(angular=3.0)
state = SPIN
spinStart = time.time()
elif state == APPROACH_ORIENT:
if ct.waitingForCommand():
state = APPROACH_GO
ct.setBasicControl(forward=3.0)
elif state == APPROACH_GO:
if len(knownBalls) != 0:
state = GRAB
print "Heading to grab ball"
ct.setWayPointControl()
ct.addWayPoints([[a[0],a[1],0,False] for a in knownBalls])
elif not ct.movingForward():
state = FOLLOW
ct.setWallFollowControl()
elif state == SPIN:
if len(knownBalls) != 0:
state = GRAB
ct.setWayPointControl()
ct.addWayPoints([[a[0],a[1],0,False] for a in knownBalls])
elif len(balls) != 0:
state = APPROACH_ORIENT
ct.setWayPointControl()
ct.addWayPoints([[pose[0],pose[1],v.getAreaAngle(balls[0],pose),True]])
elif time.time()-spinStart >= spinDuration:
ct.setWallFollowControl()
state = FOLLOW
time.sleep(max(0,1.0/freq - (time.time()-start)))
cleanQuit('','')