def update(stop = False, rotate=False):
global pose,sensorPoints
#-------------------------Receive Data from Arduino
# data is [Left Encoder, Right Encoder]
data = ser.receiveData()
#-------------------------Update Odometry
pose = odo.update(data[0],data[1])
#print data, pose
#-------------------------Update Sensor Values
sensorPoints = sensor.update(data[2:7],pose)
#-------------------------Debug Print
[forSet,angSet] = waypointNav.update(pose)
if abs(angSet) > 3.5:
angSet = 3.5*abs(angSet)/angSet
if abs(forSet) > 3.5:
forSet = 3.5*abs(forSet)/forSet
if rotate:
mot.setAngForVels(0,3.5)
else:
mot.setAngForVels(forSet,angSet)
#mot.setAngForVels(.5,0)
[dThetaLdt,dThetaRdt] = odo.getVel()
[lCommand,rCommand] = mot.update(dThetaLdt,dThetaRdt)
if debug:
print pose,sensorPoints
if(stop):
ser.sendCommand(mot.getMotorCommandBytes(0,0))
else:
ser.sendCommand(mot.getMotorCommandBytes(lCommand,rCommand))
def update(stop=False):
global pose, sensorPoints
# -------------------------Receive Data from Arduino
# data is [Left Encoder, Right Encoder]
data = ser.receiveData()
# -------------------------Update Odometry
pose = odo.update(data[0], data[1])
"""
#-------------------------Update Sensor Values
#sensorPoints = sensor.update(data[2:7],pose)
#print sensorPoints
if waypointNav.state == waypointNav.TRANSLATING:
for i in sensorPoints[1:6]:
if ((pose[0]-i[0])**2 + (pose[1]-i[1])**2) < 9.0**2:
waypointNav.wp = []
waypointNav.addWaypoint([pose[0],pose[1],pose[2],False])
"""
# ------------------------- Update Waypoint Navigator
[forSet, angSet] = waypointNav.update(pose)
if abs(forSet) > 3:
forSet = 3 * abs(forSet) / forSet
if abs(angSet) > 3:
angSet = 3 * abs(angSet) / angSet
if waypointNav.active:
mot.setAngForVels(forSet, angSet)
# ------------------------- Update Motor Controller
[dThetaLdt, dThetaRdt] = odo.getVel()
[lCommand, rCommand] = mot.update(dThetaLdt, dThetaRdt)
# print "x = "+str(pose[0])+", y = "+str(pose[1])+", theta = "+str(math.degrees(pose[2]))
if stop:
ser.serCont.send("STOPS")
else:
s = mot.getMotorCommandBytes(lCommand, rCommand)
ser.sendCommand(s)