def navigateRobot():
navigate.init()
mag_q = multiprocessing.Queue()
scanProcess = Process(target=smallSentry, args=(mag_q, ))
scanProcess.daemon = True
scanProcess.start()
try:
while True:
distance = navigate.readDistance()
angle = navigate.calcAngle(distance)
navigate.turnTowards(angle)
if navigate.isMagnet():
#print "Should go into sentry mode. If you see this, it is commented out"
print "Found magnet. Going into sentry mode."
mag_q.put('Stop')
scanProcess.join()
return -1
if navigate.isLeftWhite():
initial = distance[3]
while (navigate.isLeftWhite()):
navigate.turnInPlace(-1)
navigate.turnAngle(20)
while (distance[3] > initial / 2):
navigate.turnTowards(0)
distance = navigate.readDistance()
if min(distance) < MIN_DISTANCE: break
if navigate.isLeftWhite() or navigate.isRightWhite(): break
if navigate.isRightWhite():
initial = distance[1]
while (navigate.isRightWhite()):
navigate.turnInPlace(1)
navigate.turnAngle(-20)
while (distance[1] > initial / 2):
navigate.turnTowards(0)
distance = navigate.readDistance()
if min(distance) < MIN_DISTANCE: break
if navigate.isLeftWhite() or navigate.isRightWhite(): break
if distance[2] < MIN_DISTANCE:
navigate.turnAngle(US_ANGLE[distance.index(max(distance))])
finally:
#GPIO.cleanup()
scanProcess.join()
print "Ran the navigate close"
def navigateRobot():
try:
navigate.init()
while True:
distance = navigate.readDistance()
angle = navigate.calcAngle(distance)
navigate.turnTowards(angle)
if navigate.isMagnet():
#print "Should go into sentry mode. If you see this, it is commented out"
print "Going into sentry mode."
sentryMode()
return -1
if navigate.isLeftWhite():
initial = distance[3]
while(navigate.isLeftWhite()): navigate.turnInPlace(-1)
navigate.turnAngle(20)
while (distance[3] > initial/2):
navigate.turnTowards(0)
distance = navigate.readDistance()
if min(distance) < MIN_DISTANCE: break
if navigate.isLeftWhite() or navigate.isRightWhite(): break
if navigate.isRightWhite():
initial = distance[1]
while(navigate.isRightWhite()): navigate.turnInPlace(1)
navigate.turnAngle(-20)
while (distance[1] > initial/2):
navigate.turnTowards(0)
distance = navigate.readDistance()
if min(distance) < MIN_DISTANCE: break
if navigate.isLeftWhite() or navigate.isRightWhite(): break
if distance[2] < MIN_DISTANCE:
navigate.turnAngle(US_ANGLE[distance.index(max(distance))])
finally:
#GPIO.cleanup()
print "Ran the navigate close"
def navigateBack():
print "Navigating back."
navigate.init()
nav_q = multiprocessing.Queue()
scan_q = multiprocessing.Queue()
scanProcess = Process(target = scan, args = (nav_q,scan_q,))
scanProcess.daemon = True
scanProcess.start()
targetAngle = None
currentAngle = 0
currentTime = time.time()
lastTime = time.time()
Error = False
while targetAngle != 'STOP':
distance = navigate.readDistance()
if abs(currentAngle) > 360:
#there was a problem, just wait for a new bearing
targetAngle = None
currentAngle = 0
if targetAngle != None:
# Read the current angle
currentTime = time.time()
dTime = currentTime - lastTime
lastTime = currentTime
if not Error:
currentAngle = currentAngle - navigate.readGyro() * dTime
# calculate delta angle between current and target
dAngle = targetAngle - currentAngle
if dAngle < -180:
currentAngle += 360
dAngle = targetAngle - currentAngle
if dAngle > 180:
currentAngle -= 360
dAngle = targetAngle - currentAngle
#navigate based on delta angle
print currentAngle, targetAngle, dAngle
if abs(dAngle) < 180:
scale = (180 - abs(dAngle))/180.0
if dAngle < 0:
distance[3] *= scale
distance[4] *= scale
if distance[3] < 10: distance[3] = 10
if distance[4] < 10: distance[4] = 10
elif dAngle > 0:
distance[0] *= scale
distance[1] *= scale
if distance[0] < 10: distance[0] = 10
if distance[1] < 10: distance[1] = 10
angle = navigate.calcAngle(distance)
navigate.turnTowards(angle)
Error = False
'''
elif abs(dAngle) > 90:
scan_q.put(dAngle)
navigate.turnAngle(dAngle)
currentAngle += dAngle
Error = True
'''
if min(distance) < 6:
#find if left or right too close
print "Emergency turn"
if distance.index(min(distance)) > 2:
navigate.turnAngle(-20)
currentAngle += -20
scan_q.put(-20)
if distance.index(min(distance)) < 2:
navigate.turnAngle(20)
currentAngle += 20
scan_q.put(20)
time.sleep(0.5)
Error = True
try:
targetAngle = nav_q.get(timeout = 0.001)
print "Target angle:", targetAngle
currentAngle = 0
except:
pass
if targetAngle == None:
angle = navigate.calcAngle(distance)
navigate.turnTowards(angle)
if navigate.isLeftWhite():
initial = distance[3]
while(navigate.isLeftWhite()): navigate.turnInPlace(-1)
scan_q.put(20)
navigate.turnAngle(20)
currentAngle += 20
Error = True
while (distance[3] > initial/2):
navigate.turnTowards(0)
distance = navigate.readDistance()
if min(distance) < MIN_DISTANCE: break
if navigate.isLeftWhite() or navigate.isRightWhite(): break
if navigate.isRightWhite():
initial = distance[1]
while(navigate.isRightWhite()): navigate.turnInPlace(1)
scan_q.put(-20)
navigate.turnAngle(-20)
currentAngle -= 20
Error = True
while (distance[1] > initial/2):
navigate.turnTowards(0)
distance = navigate.readDistance()
if min(distance) < MIN_DISTANCE: break
if navigate.isLeftWhite() or navigate.isRightWhite(): break
if distance[2] < MIN_DISTANCE:
currentAngle += US_ANGLE[distance.index(max(distance))]
scan_q.put(US_ANGLE[distance.index(max(distance))])
navigate.turnAngle(US_ANGLE[distance.index(max(distance))])
Error = True
scanProcess.join()