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 update(stop = False):
global pose,sensorData,updateLock,CONTROLLER,basicFor,basicAng
#-------------------------Receive Data from Arduino
# data is [Left Encoder, Right Encoder,
# Sensor 0 (leftmost), Sensor 1, Sensor 2 (center, forward),
# Sensor 3, Sensor 4 (rightmost)]
data = ser.receiveData()
#-------------------------Update Odometry
pose = odo.update(data[0],data[1])
#-------------------------Store sensor data
sensorData = list(data[2:8])
sp = getSensorPoints()
for s in sp[1:4]:
if distance(pose,s) < ROBOT_RADIUS+3:
if(wpNav.state == wpNav.TRANSLATING):
clearWayPoints()
basicFor.value = 0.0
if wf.state == wf.FOLLOW:
wf.reset()
#-------------------------Get forward and angular velocities
# Can get them from either a basic (forward,angular) velocity
# controller, or from a waypoint navigator.
# Synchronized through locking because modifications will only affect this part
if(CONTROLLER.value == WAYPOINT):
[forSet,angSet] = wpNav.update(pose)
if len(wpNav.wp) == 0:
inWait.value = 1
else:
inWait.value = 0
elif(CONTROLLER.value == BASIC):
inWait.value = 1
[forSet,angSet] = basicFor.value,basicAng.value
elif(CONTROLLER.value == WALL_FOLLOW):
[forSet,angSet] = wf.update(sp,pose)
elif(CONTROLLER.value == WALL_ALIGN):
[forSet,angSet] = wa.update([distance(pose,s) for s in sp])
if wa.isDone():
inWait.value = 1
else:
inWait.value = 0
if forSet == 0:
forMove.value = 0
else:
forMove.value = 1
#-------------------------Limit speeds according to empirical results
if abs(forSet) > MAX_FOR_SPEED:
forSet = math.copysign(MAX_FOR_SPEED,forSet)
if abs(angSet) > MAX_ANG_SPEED:
angSet = math.copysign(MAX_ANG_SPEED,angSet)
mot.setAngForVels(forSet,angSet)
#-------------------------Use motor controller to get motor commands
[dThetaLdt,dThetaRdt] = odo.getVel()
[lCommand,rCommand] = mot.update(dThetaLdt,dThetaRdt)
#-------------------------Send commands (send STOPS command if update is told to stop)
if(stop):
ser.serCont.send('STOPS')
else:
ser.sendCommand(mot.getMotorCommandBytes(lCommand,rCommand))
#-------------------------Debug Print
if debug:
print data,pose,sensorData,forSet,angSet,lCommand,rCommand