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()