コード例 #1
0
ファイル: control.py プロジェクト: teamhex/teamhex
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()
コード例 #2
0
ファイル: control.py プロジェクト: teamhex/teamhex
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)))
コード例 #3
0
ファイル: control.py プロジェクト: teamhex/teamhex
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('','')