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)
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)
def testWaypoints(): initialize() waypointNav.addWaypoints([[24,0,0],[24,24,math.pi/2.0],[0,24,math.pi],[0,0,0]]) while(True): update()