Beispiel #1
0
def controlLoop(freq=100):
    global pose,sensorData,stopCommand,sharedArray
    while stopCommand.value == 0:
        start = time.time()
        while not commandQueue.empty():
            com = commandQueue.get()
            if(com == ACTIVATE):
                wpNav.activate()
            elif(com == DEACTIVATE):
                wpNav.deactivate()
            elif(com == CLEAR):
                wpNav.clearWaypoints()
            elif(com == FOLLOW_START):
                wf.reset()
            elif(com == RAMP_CHANGE):
                ser.receiveData()
                ser.serCont.send('RAMPCH')
            elif(com == SHOOTER_CHANGE):
                ser.receiveData()
                ser.serCont.send('SHOOTER')
            elif(com == ADD_WPS):
                while not wpQueue.empty():
                    wpNav.addWaypoints(wpQueue.get())
            elif(com == ALIGN_RESET):
                wa.reset()
                
        update()
        sharedArray[:] = [pose[0],pose[1],pose[2]] + sensorData
        time.sleep(max(0,1/float(freq) - (time.time()-start)))
    update(stop=True)
Beispiel #2
0
def initialize():
    ser.initialize()
    wpNav.initialize()
    wf.initialize()
    odo.initialize(realWidth/2.0, realHeight/2.0)
    
    wpNav.clearWaypoints()
    wpNav.deactivate()
    CONTROLLER.value = WAYPOINT
    basicFor.value = basicAng.value = 0.0
    stopCommand.value = 0
    inWait.value = 1

    p = mp.Process(target=controlLoop)
    p.start()
Beispiel #3
0
def controlLoop(freq=200):
    global pose,sensorData,stopCommand,sharedArray
    while stopCommand.value == 0:
        start = time.time()
        while not wpQueue.empty():
            wpNav.addWaypoints(wpQueue.get())
        while not commandQueue.empty():
            com = commandQueue.get()
            if(com == ACTIVATE):
                wpNav.activate()
            elif(com == DEACTIVATE):
                wpNav.deactivate()
            elif(com == CLEAR):
                wpNav.clearWaypoints()
            elif(com == FOLLOW_START):
                wf.reset()
        update()
        sharedArray[:] = [pose[0],pose[1],pose[2]] + sensorData
        time.sleep(max(0,1/float(freq) - (time.time()-start)))
    update(stop=True)