예제 #1
0
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"
예제 #2
0
파일: MAIN.py 프로젝트: GriffToy/Robot
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"
예제 #3
0
파일: MAIN.py 프로젝트: GriffToy/Robot
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()