Exemplo n.º 1
0
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)))
Exemplo n.º 2
0
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)))
Exemplo n.º 3
0
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
Exemplo n.º 4
0
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)
Exemplo n.º 5
0
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
Exemplo n.º 6
0
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('','')