def update():
global pose
#-------------------------Receive Data from Arduino
# data is [Left Encoder, Right Encoder]
data = ser.receive()
#print data
#-------------------------Update Odometry
pose = odo.update(data[0],data[1])
# if((pose != odo.pose) and debug):
if debug:
print "x = "+str(pose[0])+", y = "+str(pose[1])+", theta = "+str(math.degrees(pose[2]))
#-------------------------Update Waypoint Navigator
# [forSet,angSet] = waypointNav.update(pose)
#mot.setAngForVels(forSet,angSet)
#-------------------------Update Motor Controller
[dThetaLdt, dThetaRdt] = odo.getVel()
[lCommand, rCommand] = mot.update(dThetaLdt, dThetaRdt)
#print lCommand,rCommand
ser.sendCommand(mot.getMotorCommandBytes(lCommand,rCommand))
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
def cleanQuit(signal, frame):
print "Interrupt received"
ser.sendCommand(mot.getMotorCommandBytes(0,0))
ser.serEnc.stop()
ser.serMot.stop()
sys.exit(0)
def initialize():
ser.initialize(encPort = '/dev/arduino_encoders',motPort = '/dev/arduino_control', myBaud = 1000000)
#waypointNav.initialize()
ser.sendCommand(mot.getMotorCommandBytes(0,0))