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 goMapping(freq=30):
global myMap,q
# Parameters
RED_BALL = 4
GREEN_BALL = 5
YELLOW_WALL = 2
myMap.initMapping()
while not q:
start = time.time()
sensorPoints = ct.getSensorPoints()
pose = ct.getPose()
balls = getBalls()
knownBalls = [v.getBallCoords(b,pose) for b in getKnownBalls(balls)]
specialWallPoints = [getPoint(pose,v.getAreaAngle(a,pose),640) for a in getWalls()]
# Mapping update
myMap.robotPositioned(ctypes.c_double(pose[0]), ctypes.c_double(pose[1]))
for a in knownBalls:
myMap.ballDetected(ctypes.c_double(a[0]),ctypes.c_double(a[1]),ctypes.c_int(RED_BALL))
for wp in specialWallPoints:
myMap.specialWall(ctypes.c_double(wp[0]),ctypes.c_double(wp[1]),ctypes.c_int(YELLOW_WALL))
for s in sensorPoints:
if(s[2]):
myMap.wallDetected(ctypes.c_double(s[0]), ctypes.c_double(s[1]))
else:
myMap.wallNotDetected(ctypes.c_double(s[0]), ctypes.c_double(s[1]))
time.sleep(max(0,1.0/float(freq) - (time.time()-start)))
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 getKnownBalls(balls):
pose = ct.getPose()
knownBalls = []
for b in balls:
bc = v.getBallCoords(b,pose)
if bc is not None:
knownBalls.append(b)
return knownBalls
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)
def ballGoAndRotate(endTime,freq=10.0):
ballGo(endTime)
while not ct.waitingForCommand() and (endTime > time.time()):
time.sleep(1.0/freq)
initial = ct.getPose()[2]
prev = 0
ct.setBasicControl(angular=3.0)
while (ct.getPose()[2]-initial)%(2*math.pi) >= prev and (endTime > time.time()):
startTime = time.time()
if len(getBalls()) > 0:
ballGo(endTime)
while not ct.waitingForCommand():
time.sleep(1.0/freq)
inital = ct.getPose()[2]
ct.setBasicControl(angular=2.0)
prev = (ct.getPose()[2]-initial)%(2*math.pi)
time.sleep(max(0,1.0/freq-(time.time()-startTime)))
ct.setBasicControl()
def getBalls():
myMap.setConfigSpace()
balls = []
pose = ct.getPose()
for b in v.getAreas():
if v.isBall(b):
bc = v.getBallCoords(b,pose)
theta = v.getAreaAngle(b,pose)
if (bc is None and canDrive(pose,getPoint(pose,theta,16))) or (bc is not None and canDrive(pose,bc)):
balls.append(b)
return balls
def scoreGo(endTime, freq=10.0):
pose = ct.getPose()
walls = getWalls()
vsTroller = PIDController(0.05,.001,.02)
vsTroller.setDesired(0)
if len(walls) > 0:
print "Going to the wall :)"
wall = walls[0]
ct.setBasicControl(forward=0.1)
while ct.movingForward() and (endTime > time.time()):
walls = getWalls()
if len(walls) > 0:
wall = getWalls()[0]
error = wall.centerC - v.xRes/2.0
else:
error = 0
ct.setBasicControl(angular = vsTroller.update(error), forward = 3.0)
time.sleep(1.0/freq)
ct.setWallAlignmentControl()
while not ct.waitingForCommand() and (endTime > time.time()):
time.sleep(1.0/freq)
if(endTime > time.time()):
score()
else:
# Spin 360 degrees looking for wall
initial = ct.getPose()[2]
prev = 0
ct.setBasicControl(angular=3.0)
while (ct.getPose()[2]-initial)%(2*math.pi) >= prev and (endTime > time.time()):
startTime = time.time()
if len(getWalls()) > 0:
scoreGo(endTime)
break
prev = (ct.getPose()[2]-initial)%(2*math.pi)
time.sleep(max(0,1.0/freq-(time.time()-startTime)))
ct.setBasicControl()
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('','')