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