Exemplo n.º 1
0
def wall_follow_demo():
    velocity.init(0.22, 40, 0.5, 0.1)
    leds.init()
    pose.init()
    motion.init()
    neighbors.init(NBR_PERIOD)

    state = STATE_IDLE
    wall_time = 0
    
    while True:
        # Do updates
        leds.update()
        pose.update()
        velocity.update()
        new_nbrs = neighbors.update()
        
        nbrList = neighbors.get_neighbors()
        tv = 0
        rv = 0

        # this is the main finite-state machine
        if state == STATE_IDLE:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            if new_nbrs:
                print "idle"
            if rone.button_get_value('r'):
                state = STATE_LOOK_FOR_WALL
            
        elif state == STATE_LOOK_FOR_WALL:
            leds.set_pattern('r', 'blink_fast', LED_BRIGHTNESS)
            if new_nbrs:
                print "look for wall"
            tv = MOTION_TV
            obs = neighbors.get_obstacles() 
            if (obs != None):
                state = STATE_WALL_FOLLOW                
            
        elif state == STATE_WALL_FOLLOW:
            leds.set_pattern('b', 'blink_fast', LED_BRIGHTNESS)
            if new_nbrs:
                print "wall follow"
            # follow the wall
            (tv, rv, active) = wall_follow(MOTION_TV / 2)
            if active == True:
                wall_time = sys.time()
            if sys.time() > (wall_time + WALL_TIMEOUT):
                state = STATE_LOOK_FOR_WALL
                
        # end of the FSM
                        
        # set the velocities
        velocity.set_tvrv(tv, rv)
        
        #set the message
        hba.set_msg(0, 0, 0)
Exemplo n.º 2
0
Arquivo: beh.py Projeto: kgmstwo/THBCP
def update():
    leds.update()
    pose.update()
    motion.update()
    velocity.update()
    return neighbors.update()
Exemplo n.º 3
0
def update():
    leds.update()
    pose.update()
    motion.update()
    velocity.update()
    return neighbors.update()