def update(sensorPoints,pose):
global state
global desiredDist
global timeoutStart,lostTimeout
if state == FIND:
for i,s in enumerate(sensorPoints[:-1]):
if s[2] and distance(pose,s) <= desiredDist:
wpNav.initialize()
wpNav.clearWaypoints()
wpNav.addWaypoint([pose[0],pose[1],pose[2]+(sen.angArray[i]-sen.angArray[-1]),True])
print "Aligning with the wall (using waypoint navigator)"
state=ORIENT
return wpNav.update(pose)
return [6.0,0.0]
elif state == ORIENT:
if wpNav.state != wpNav.IDLE:
return wpNav.update(pose)
else:
distAngTroller.setDesired(desiredDist,reset=True)#distance(sensorPoints[0],pose),reset=True)
print "Found and aligned with wall, following now."
state = FOLLOW
return [0,0]
elif state == FOLLOW:
sp = sensorPoints
if not sp[-1][2] and not sp[-2][2]:
print "Lost wall, trying to find it again."
timeoutStart = time.time()
state = LOST_TIMEOUT
return [5.0,-2.0]
elif not sp[-2][2]:
dist = distance(sp[-1],pose)
elif not sp[-1][2]:
dist = distance(sp[-2],pose)
else:
ax,ay = sp[-2][0]-sp[-1][0],sp[-2][1]-sp[-1][1]
bx,by = pose[0]-sp[-1][0],pose[1]-sp[-1][1]
x = distance(pose,sp[-1])
dist = x*math.sqrt(1- (ax*bx + ay*by)/(distance(sp[-1],sp[-2])*x))
return [5.0,distAngTroller.update(dist)]
elif state == LOST_TIMEOUT:
if sensorPoints[-1][2] or sensorPoints[-2][2]:
print "Found wall again, back to following it."
distAngTroller.setDesired(desiredDist,reset=True)
state = FOLLOW
return [0,0]
elif (time.time()-timeoutStart) > lostTimeout:
state=FIND
return [5.0,-2.0]
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.receiveEnc()
#-------------------------Update Odometry
pose = odo.update(data[0],data[1])
#-------------------------Store sensor data
sensorData = ser.receiveSensors(pose)
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)
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)
ser.motSetAngForVels(forSet,angSet)
#-------------------------Debug Print
if debug:
print data,pose,sensorData,forSet,angSet,'\n'
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
