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 initialize():
ser.initialize()
wpNav.initialize()
wf.initialize()
odo.initialize(realWidth/2.0, realHeight/2.0)
pose = [realWidth/2.0, realHeight/2.0, 0.]
wpNav.clearWaypoints()
wpNav.deactivate()
wa.reset()
CONTROLLER.value = BASIC
basicFor.value = basicAng.value = 0.0
stopCommand.value = 0
inWait.value = 1
p = mp.Process(target=controlLoop)
p.start()