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