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 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 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('','')