Ejemplo n.º 1
0
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]
Ejemplo n.º 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()