Exemple #1
0
def flower():
    beh.init(0.22, 40, 0.5, 0.1)
    flower_type = TYPE_RED
    color = 'r'

    # Broadcast the ID with a message saying that it is a flower
    while True:
        # Do updates
        newNbrs = beh.update()
        beh_out = beh.BEH_INACTIVE

        if rone.button_get_value('r'):
            flower_type = TYPE_RED
            color = 'r'
        if rone.button_get_value('g'):
            flower_type = TYPE_GREEN
            color = 'g'
        if rone.button_get_value('b'):
            flower_type = TYPE_BLUE
            color = 'b'

        hba.set_msg(0, 0, flower_type)
        leds.set_pattern(color, 'ramp_slow', LED_BRIGHTNESS)

        beh.motion_set(beh_out)
Exemple #2
0
def flower():
    beh.init(0.22, 40, 0.5, 0.1)
    flower_type = TYPE_RED;
    color = 'r'
    
    # Broadcast the ID with a message saying that it is a flower
    while True:
        # Do updates
        newNbrs = beh.update()
        beh_out = beh.BEH_INACTIVE

        if rone.button_get_value('r'):
            flower_type = TYPE_RED
            color = 'r'
        if rone.button_get_value('g'):
            flower_type = TYPE_GREEN
            color = 'g'
        if rone.button_get_value('b'):
            flower_type = TYPE_BLUE
            color = 'b'

        hba.set_msg(0, 0, flower_type)
        leds.set_pattern(color, 'ramp_slow', LED_BRIGHTNESS)
        
        beh.motion_set(beh_out)
Exemple #3
0
def flock_demo():
    mode = MODE_IDLE

    beh.init(0.22, 40, 0.5, 0.1)

    buttons = hba.wait_for_button()
    
    if 'r' in buttons:
        mode = MODE_REMOTE
    elif 'g' in buttons:
        mode = MODE_FLOCK
    
    # Now that you know your mode, run the main loop
    while True:
        # run the system updates
        new_nbrs = beh.update()

        beh_out = beh.BEH_INACTIVE
        if mode == MODE_REMOTE:                
            buttons = hba.check_buttons()
        
            if buttons != '':
                leds.set_pattern('r','blink_fast',LED_BRIGHTNESS)
                rone.radio_send_message(buttons)
            else:
                leds.set_pattern('r','circle',LED_BRIGHTNESS)
            #sleep for a bit to avoid continuous radio transmission
            sys.sleep(REMOTE_XMIT_DELAY)
        elif mode == MODE_FLOCK:
            bump_beh_out = beh.bump_beh(FTL_TV)
            receive_beh_out = receive_beh()
            flock_beh_out = flock_beh()
        
#            if beh.get_active(bump_beh_out):
#                tv = beh.get_tv(bump_beh_out)
#                rv = beh.get_rv(bump_beh_out)
#            else:
#                tv = beh.get_tv(receive_beh_out) + beh.get_tv(flock_beh_out)
#                rv = beh.get_rv(receive_beh_out) + beh.get_rv(flock_beh_out)

            flock_beh_out = beh.sum(flock_beh_out, receive_beh_out)
            beh_out = beh.subsume([flock_beh_out, bump_beh_out])
            
        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        hba.set_msg(0, 0, 0)
Exemple #4
0
def nav_tower_motion():
    beh.init(0.22, 40, 0.5, 0.1)
    state = STATE_MOVE_TO_TOWER
    motion_start_odo = pose.get_odometer()
    while True: #while True loop just for testing, will need to call many times
        nav_tower = hba.find_nav_tower_nbr(127)
        if state == STATE_MOVE_TO_TOWER:
            new_nbrs = beh.update()
        # Move towards the nav_tower until turning around distance reached
            if nav_tower != None:      # move forward
                beh_out = beh.follow_nbr(nav_tower, MOTION_TV)
                leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
            else:
                leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
                beh_out = beh.follow_nbr(nav_tower, MOTION_TV)  
            distance_to_go = (motion_start_odo + MOVE_TO_TOWER_DISTANCE) - pose.get_odometer()
            beh.motion_set(beh_out)
            if distance_to_go < 0:    
                state = STATE_IDLE   
    return
Exemple #5
0
def nav_tower_motion():
    beh.init(0.22, 40, 0.5, 0.1)
    state = STATE_MOVE_TO_TOWER
    motion_start_odo = pose.get_odometer()
    while True:  #while True loop just for testing, will need to call many times
        nav_tower = hba.find_nav_tower_nbr(127)
        if state == STATE_MOVE_TO_TOWER:
            new_nbrs = beh.update()
            # Move towards the nav_tower until turning around distance reached
            if nav_tower != None:  # move forward
                beh_out = beh.follow_nbr(nav_tower, MOTION_TV)
                leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
            else:
                leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
                beh_out = beh.follow_nbr(nav_tower, MOTION_TV)
            distance_to_go = (motion_start_odo +
                              MOVE_TO_TOWER_DISTANCE) - pose.get_odometer()
            beh.motion_set(beh_out)
            if distance_to_go < 0:
                state = STATE_IDLE
    return
Exemple #6
0
def summer():
    beh.init(0.22, 40, 0.5, 0.1)

    state = STATE_IDLE

    while True:
        # run the system updates
        new_nbrs = beh.update()

        nbrList = neighbors.get_neighbors()
        if new_nbrs:
            print nbrList
        beh_out = beh.BEH_INACTIVE

        # this is the main finite-state machine
        if state == STATE_IDLE:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            leds.set_pattern('b', 'circle', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_FIND_QUEEN
            if rone.button_get_value('b'):
                state = STATE_QUEEN
            if new_nbrs:
                print "idle"
        elif state == STATE_FIND_QUEEN:
            leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
            beh_out = beh.tvrv(MOTION_TV, 0)
            queen = get_queen()
            if not queen == None:
                state = STATE_BUMP_QUEEN
            else:
                #go straight and hope for the best
                beh_out = beh.tvrv(MOTION_TV, 0)
        elif state == STATE_BUMP_QUEEN:
            leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
            queen = get_queen()
            if queen == None:
                state = STATE_RETURN
            else:
                if (neighbors.get_nbr_range_bits(queen) >
                        6) or (beh.bump_angle_get() != None):
                    state = STATE_BACK_UP
                    start_time = sys.time()
                else:
                    beh_out = beh.follow_nbr(queen, MOTION_TV)
        elif state == STATE_BACK_UP:
            if sys.time() > start_time + BACK_UP_TIME:
                state = STATE_RETURN
            else:
                beh_out = beh.tvrv(-MOTION_TV, 0)
        elif state == STATE_RETURN:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_FIND_QUEEN
            queen = get_queen()
            if queen == None:
                state = STATE_IDLE
            else:
                beh_out = beh.avoid_nbr(queen, MOTION_TV)
        elif state == STATE_QUEEN:
            if new_nbrs:
                print 'Ich bin die Koenigin der welt!'

        # end of the FSM
        bump_beh_out = beh.bump_beh(MOTION_TV)
        if not state == STATE_QUEEN:
            beh_out = beh.subsume([beh_out, bump_beh_out])

        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        hba.set_msg(state, 0, 0)
Exemple #7
0
def message_demo():
    beh.init(0.22, 40, 0.5, 0.1)

    state = STATE_LEADER

    num_r = 1
    num_g = 1
    num_b = 1
    button_red_old = 0
    button_green_old = 0
    button_blue_old = 0

    while True:
        # run the system updates
        new_nbrs = beh.update()
        nbr_list = neighbors.get_neighbors()

        # init the velocities
        beh_out = beh.BEH_INACTIVE

        # read the buttons and update the local counts
        (num_r, button_red_old) = check_button('r', num_r, button_red_old)
        (num_g, button_green_old) = check_button('g', num_g, button_green_old)
        (num_b, button_blue_old) = check_button('b', num_b, button_blue_old)

        # build this robot's neighbor message
        distance_local = num_r
        mode_local = num_g
        quality_local = num_b
        hba.set_msg(distance_local, mode_local, quality_local)

        # find the neighbor with the lowest ID
        nbr_leader = nbrList_getRobotWithLowID(nbr_list)

        # are you the leader?
        if nbr_leader == None:
            # no neighbors.  you are the leader
            state = STATE_LEADER
        else:
            low_ID = neighbors.get_nbr_id(nbr_leader)
            #            if new_nbrs:
            #                print 'nbr lowID=',nbr_leader
            if low_ID < rone.get_id():
                state = STATE_MINION
            else:
                state = STATE_LEADER

        # this is the main finite-state machine
        if state == STATE_LEADER:
            msg = (distance_local, mode_local, quality_local)
            if new_nbrs:
                print "leader:", msg
            brightness = LED_BRIGHTNESSER

        elif state == STATE_MINION:
            if nbr_leader != None:
                msg = hba.get_msg_from_nbr(nbr_leader, new_nbrs)
                if new_nbrs:
                    print "minion. leader = ", str(low_ID), " msg=", msg
            brightness = LED_BRIGHTNESS

        # end of the FSM
        # unpack the message and display it on the LEDs
        (distance, mode, quality) = msg
        leds.set_pattern((distance, mode, quality), 'count', brightness)

        # set the velocities
        beh.motion_set(beh_out)
Exemple #8
0
def fall():
    found_flower = False
    start_time = 0
    target_theta = 0
    my_color = -1
    beh.init(0.22, 40, 0.5, 0.1)
    state = STATE_IDLE
    color = 'r'  #for flowers only

    def wander():
        state = STATE_WANDER

    def collect_pollen():
        state = STATE_COLLECT_POLLEN
        start_time = sys.time()

    def align_with(target):
        target_theta = target
        pose.set_pose(0, 0, 0)
        state = STATE_ALIGN
        start_time = sys.time()

    #motion_start_odo = pose.get_odometer()

    while True:
        beh.init(0.22, 40, 0.5, 0.1)

        new_nbrs = beh.update()
        nbrList = neighbors.get_neighbors()
        if new_nbrs:
            print nbrList
        beh_out = beh.BEH_INACTIVE

        #FINITE STATE MACHINE
        if state == STATE_IDLE:
            leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_MOVE_TO_FLOWER
            if rone.button_get_value('b'):
                state = STATE_QUEEN
            if new_nbrs:
                print "idle"

        elif state == STATE_WANDER:  #run forward, avoid direction of neighbors
            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            if nav_tower == None:
                state = STATE_RETURN_TO_BASE
            else:
                beh_out = beh.avoid_nbr(nav_tower, MOTION_TV)

                (flower, color) = detflower(nbrList)
                if flower != None:
                    state = STATE_MOVE_TO_FLOWER

        elif state == STATE_MOVE_TO_FLOWER:
            leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
            (flower, color) = detflower(nbrList)
            if flower != None:
                if (neighbors.get_nbr_range_bits(flower) >
                        6) or (beh.bump_angle_get() != None):
                    collect_pollen()  #collect pollen if we bump or get close
                else:
                    #otherwise keep following that flower
                    beh_out = beh.follow_nbr(flower, MOTION_TV)

        elif state == STATE_COLLECT_POLLEN:
            motion_start_odo = pose.get_odometer()
            if sys.time() > (collect_pollen_start_time + COLLECT_POLLEN_TIME):
                state = STATE_RETURN_TO_BASE
                found_flower = True
            elif sys.time() < (collect_pollen_start_time + BACK_UP_TIME):
                tv = -MOTION_TV
                rv = 0
                beh_out = beh.tvrv(tv, rv)
                turn_start_time = (collect_pollen_start_time + BACK_UP_TIME)

            elif sys.time() < (turn_start_time + TURN_TIME):
                tv = 40
                rv = -MOTION_RV
                beh_out = beh.tvrv(tv, rv)

            else:
                tv = MOTION_TV
                rv = (MOTION_RV - 300)
                beh_out = beh.tvrv(tv, rv)

        elif state == STATE_RETURN_TO_BASE:
            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            queen = find_queen(nbrList)
            if (nav_tower == None) and (queen == None):
                beh_out = (-MOTION_TV, 0, True)
            elif (nav_tower != None) and (queen == None):
                beh_out = beh.follow_nbr(nav_tower)
            elif neighbors.get_nbr_range_bits(queen) > 2:
                beh_out = beh.follow_nbr(queen, MOTION_TV)
            elif found_flower:
                state = STATE_RECRUIT
                start_time = sys.time()
            else:
                state = STATE_FOLLOW
                start_time = sys.time()

        elif state == STATE_FOLLOW:
            recruiter = find_recruiter()
            if recruiter == None:
                beh_out = beh.BEH_INACTIVE
                if sys.time() > (follow_start_time + FOLLOW_TIME):
                    wander()
            else:
                bearing = neighbors.get_nbr_bearing(recruiter)
                orientation = neighbors.get_nbr_orientation(recruiter)
                align_with(math.pi + bearing - orientation)

        elif state == STATE_GO:
            flower = detflower()
            if not flower == None:
                state = STATE_MOVE_TO_FLOWER
            beh_out = beh.tvrv(MOTION_TV, 0)

        elif state == STATE_RECRUIT:
            if sys.time() > (recruit_start_time + RECRUIT_TIME):
                align_with(pose.get_theta() - math.pi)

        elif state == STATE_ALIGN:
            tv = 0
            heading_error = math.normalize_angle(pose.get_theta() -
                                                 target_theta)
            rv = ROTATE_RV_GAIN * heading_error
            beh_out = beh.tvrv(tv, rv)
            # you could actually do a running average in the list here
            small_error = hba.average_error_check(heading_error, [],
                                                  HEADING_ERROR_LIMIT,
                                                  new_nbrs)
            if new_nbrs:
                print "error", error_list
            if small_error:
                state = STATE_GO
        #END OF FINITE STATE MACHINE

        bump_beh_out = beh.bump_beh(MOTION_TV)
        if state not in [
                STATE_RETURN_TO_BASE, STATE_COLLECT_POLLEN, STATE_RECRUIT
        ]:
            beh_out = beh.subsume([beh_out, bump_beh_out])
        beh.motion_set(beh_out)
        hba.set_msg(state, my_color, 0)
Exemple #9
0
def flower_motion():
    beh.init(0.22, 40, 0.5, 0.1)

    state = STATE_IDLE

    while True:
        # run the system updates
        new_nbrs = beh.update()
        
        nbrList = neighbors.get_neighbors()
        if new_nbrs:
            print nbrList
        beh_out = beh.BEH_INACTIVE
            
        # this is the main finite-state machine
        if state == STATE_IDLE:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_MOVE_TO_FLOWER
            if new_nbrs:
                print "idle"
            
        elif state == STATE_MOVE_TO_FLOWER:
            leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
            if new_nbrs:
                print "move to flower"
           
            # Move towards the flower until you bump into it
            # for this demo, assume the first robot on the list is a flower
##            flower = nbrList_getFirstRobot(nbrList)
            (color,nbr) = detflower(nbrList)
            flower = nbr
            if flower != None:
                # Stop if we get close or bump into the flower
                #if neighbors.get_nbr_close_range(flower):
                if (neighbors.get_nbr_range_bits(flower) > 6) or (beh.bump_angle_get() != None):
                    state = STATE_COLLECT_POLLEN
                    collect_pollen_start_time = sys.time()
                else:
                    # Move to the flower
                    beh_out = beh.follow_nbr(flower, MOTION_TV)
                    #print beh_out

        elif state == STATE_COLLECT_POLLEN:
            # this is where you will put your clever pollen collection code
            # we will just wait for a second, then leave. (this will not collect very much pollen)
            leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
            if new_nbrs:
                print "collect"
            
            # Timeout after 5 seconds
            if sys.time() > (collect_pollen_start_time + COLLECT_POLLEN_TIME):
                state = STATE_MOVE_AWAY_FLOWER
            
            elif sys.time() < (collect_pollen_start_time + BACK_UP_TIME):    
                tv = -MOTION_TV
                rv = 0
                beh_out = beh.tvrv(tv,rv) 
                turn_start_time = (collect_pollen_start_time + BACK_UP_TIME)
                
            elif sys.time() < (turn_start_time + TURN_TIME): 
                tv = 0
                rv = -MOTION_RV
            
            else: 
                tv = MOTION_TV
                rv = MOTION_RV
                beh_out = beh.tvrv(tv,rv)
            
        elif state == STATE_MOVE_AWAY_FLOWER:
            if new_nbrs:
                print "avoid flower"
            leds.set_pattern('r', 'blink_slow', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_MOVE_TO_FLOWER

            # Move away from the flower until it is out of range
            flower = nbrList_getFirstRobot(nbrList)
            if flower != None:
                # Point away the flower
                beh_out = beh.avoid_nbr(flower, MOTION_TV)
            else:
                state = STATE_IDLE
                
        # end of the FSM
        bump_beh_out = beh.bump_beh(MOTION_TV)

        if state != STATE_COLLECT_POLLEN:
            beh_out = beh.subsume([beh_out, bump_beh_out])

        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        hba.set_msg(0, 0, 0)
Exemple #10
0
def recruit_demo():
    beh.init(0.22, 40, 0.5, 0.1)

    state = STATE_IDLE
    
    while True:
        # run the system updates
        new_nbrs = beh.update()
        
        nbr_list = neighbors.get_neighbors()
        beh_out = beh.BEH_INACTIVE

        # this is the main finite-state machine
        if state == STATE_IDLE:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_MATCH_HEADING
                error_list = []
            if new_nbrs:
                print "idle"
                #print nbr_list
            
        elif state == STATE_MATCH_HEADING:
            leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
            if new_nbrs:
                print "match heading"
           
            # match the heading with the first robot on your nbr list
            # use the average_error_check() function to wait until you get a accurate match
            # for this demo, assume the first robot on the list is a dancing_nbr that is recruiting you to a flower
            dancing_nbr = nbrList_getFirstRobot(nbr_list)
            if dancing_nbr != None:
                # rotate to face the "dancing" robot
                tv = 0
                (rv, heading_error) = match_nbr_heading(dancing_nbr)
                beh_out = beh.tvrv(tv, rv)
                small_error = hba.average_error_check(heading_error, error_list, HEADING_ERROR_LIMIT, new_nbrs)
                if new_nbrs:
                    print "error", error_list
                if small_error:
                    # We have a good heading match.  Go get pollen!
                    state = STATE_MOVE_TO_FLOWER
                    collect_pollen_start_odo = pose.get_odometer()

        elif state == STATE_MOVE_TO_FLOWER:
            # move forward
            beh_out = beh.tvrv(MOTION_TV, 0)
            
            # stop after a fixed distance_to_go
            distance_to_go = (collect_pollen_start_odo + MOVE_TO_FLOWER_DISTANCE) - pose.get_odometer() 
            if distance_to_go < 0:
                state = STATE_IDLE

            if new_nbrs:
                print "move to flower dist:", distance_to_go
            leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)

        # end of the FSM
                        
        # set the beh velocities
        beh.motion_set(beh_out)
        
        #set the HBA message
        hba.set_msg(0, 0, 0)
Exemple #11
0
def spring():
    at_tree_odo = None
    tree_pose = None
    followers = 0
    have_seen_leader = False
    beh.init(0.22, 40, 0.5, 0.1)
    motion.init_rv(1000, MOTION_RV, MOTION_CAPTURE_DISTANCE, MOTION_RELEASE_DISTANCE
            , MOTION_CAPTURE_ANGLE, MOTION_RELEASE_ANGLE)
    state = STATE_IDLE

    while True:
        # run the system updates
        new_nbrs = beh.update()
        nbr_list = neighbors.get_neighbors()
        if new_nbrs:
            print state
            
        beh_out = beh.BEH_INACTIVE

        # set colors, because why not do it at the top
        color_counts = [0, 0, 0]
        for i in range(3):
            color_counts[i] = min([5, (state - 5 * i)])
        if state == STATE_IDLE:
            leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
        elif state == STATE_SUCCESS:
            leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
        else:
            leds.set_pattern(color_counts, 'count', LED_BRIGHTNESS)

        if rone.button_get_value('g'):
            tree_pose = None
            followers = 0
            have_seen_leader = False
            state = STATE_IDLE
            
        # this is the main finite-state machine
        if not state in [STATE_IDLE, STATE_LEADER, STATE_SUCCESS, STATE_FOLLOW]:
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state in [STATE_LEADER, STATE_SUCCESS]:
                    start_time = sys.time()
                    state = STATE_FOLLOW
                    
        if state == STATE_IDLE:

            if rone.button_get_value('r'):
                pose.set_pose(0, 0, 0)
                state = STATE_WANDER
            elif rone.button_get_value('b'):
                state = STATE_QUEEN

        elif state == STATE_QUEEN:
            pass

        elif state == STATE_WANDER:
            ##            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            nav = hba.find_nav_tower_nbr(NAV_ID)
            beh_out = beh.avoid_nbr(nav, MOTION_TV)
            queen = None
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_QUEEN:
                    queen = nbr

            if bump_front():
                if queen != None:
                    state = STATE_SUCCESS
                else:
                    tree_pose = pose.get_pose()
                    motion.set_goal((0.0, 0.0), MOTION_TV)
                    at_tree_odo = pose.get_odometer()
                    state = STATE_RETURN
            elif nav == None:
                motion.set_goal((0.0, 0.0), MOTION_TV)
                state = STATE_RETURN

        elif state == STATE_RETURN:
            ##            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            queen = None
            recruiter = None
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_QUEEN:
                    queen = nbr
                elif nbr_state == STATE_RECRUIT:
                    recruiter = nbr
            if queen != None:
                if (recruiter == None) and (tree_pose != None) and \
                close_to_nbr(queen):
                    start_time = sys.time()
                    dist_traveled = pose.get_odometer() - at_tree_odo
                    at_queen_odo = pose.get_odometer()
                    state = STATE_RECRUIT
                elif not closer_to_nbr(queen):
                    beh_out = beh.follow_nbr(queen, MOTION_TV)
                else:
                    start_time = sys.time()
                    state = STATE_FOLLOW
            else:
                (tv, rv) = motion.update()
                beh_out = beh.tvrv(tv, rv)

        elif state == STATE_RECRUIT:
            new_followers = 0
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr,new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_RECRUIT:
                    if neighbors.get_nbr_id(nbr) > rone.get_id():
                        state = STATE_FOLLOW
                elif nbr_state in [STATE_FOLLOW, STATE_QUEEN]:
                    new_followers += 1
            if new_followers > followers:
                print 'reset timer'
                start_time = sys.time()
            followers = max([followers, new_followers])
            if followers == 4 or sys.time() > start_time + WAIT_TIME:
                tree_pos = (tree_pose[0], tree_pose[1])
                motion.set_goal(tree_pos, MOTION_TV)
                state = STATE_LEADER

        elif state == STATE_FOLLOW: 
            recruiter = None
            leader = None
            success = False
            new_followers = 1
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr,new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_RECRUIT:
                    recruiter = nbr
                elif nbr_state == STATE_LEADER:
                    leader = nbr
                elif nbr_state == STATE_SUCCESS:
                    leader = nbr
                    success = True
                elif nbr_state in [STATE_FOLLOW, STATE_WANDER]:
                    new_followers += 1
            if success:
                have_seen_leader = True
            if new_followers > followers:
                start_time = sys.time()
            followers = max([followers, new_followers])

            if recruiter == None:
                if leader == None:
                    if have_seen_leader:
                        beh_out = beh.tvrv(MOTION_TV, 0)
                    elif followers == 5 or sys.time() > start_time + WAIT_TIME:
                        followers = 0
                        state = STATE_WANDER
                else:
                    if success and bump() and close_to_nbr(leader):
                        state = STATE_SUCCESS
                    beh_out = beh.follow_nbr(leader, MOTION_TV)
            
        elif state == STATE_LEADER:
##            (tv, rv) = motion.update()
##            beh_out = beh.tvrv(tv, rv)
##            if motion.is_done():
##                state = STATE_SUCCESS
            beh_out = beh.tvrv(-MOTION_TV, 0)

            if bump() or (pose.get_odometer() - at_queen_odo) > dist_traveled:
                state = STATE_SUCCESS

        elif state == STATE_SUCCESS:
            pass
        
        # end of the FSM
        
        if state not in [STATE_IDLE, STATE_RECRUIT, STATE_LEADER, STATE_SUCCESS, STATE_QUEEN]:
            bump_beh_out = beh.bump_beh(MOTION_TV)
            beh_out = beh.subsume([beh_out, bump_beh_out])

        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        msg = [0, 0, 0]
        msg[MSG_IDX_STATE] = state
        hba.set_msg(msg[0], msg[1], msg[2])
Exemple #12
0
def summer():
    beh.init(0.22, 40, 0.5, 0.1)

    state = STATE_IDLE

    while True:
        # run the system updates
        new_nbrs = beh.update()

        nbrList = neighbors.get_neighbors()
        if new_nbrs:
            print nbrList
        beh_out = beh.BEH_INACTIVE

        # this is the main finite-state machine
        if state == STATE_IDLE:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            leds.set_pattern('b', 'circle', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_FIND_QUEEN
            if rone.button_get_value('b'):
                state = STATE_QUEEN
            if new_nbrs:
                print "idle"
        elif state == STATE_FIND_QUEEN:
            leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
            beh_out = beh.tvrv(MOTION_TV, 0)
            queen = get_queen()
            if not queen == None:
                state = STATE_BUMP_QUEEN
            else:
                #go straight and hope for the best
                beh_out = beh.tvrv(MOTION_TV, 0)  
        elif state == STATE_BUMP_QUEEN:
            leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
            queen = get_queen()
            if queen == None:
                state = STATE_RETURN
            else:
                if (neighbors.get_nbr_range_bits(queen) > 6) or (beh.bump_angle_get() != None):
                    state = STATE_BACK_UP
                    start_time = sys.time()
                else:
                    beh_out = beh.follow_nbr(queen, MOTION_TV)
        elif state == STATE_BACK_UP:
            if sys.time() > start_time + BACK_UP_TIME:
                state = STATE_RETURN
            else:
                beh_out = beh.tvrv(-MOTION_TV, 0)
        elif state == STATE_RETURN:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_FIND_QUEEN
            queen = get_queen()
            if queen == None:
                state = STATE_IDLE
            else:
                beh_out = beh.avoid_nbr(queen, MOTION_TV)
        elif state == STATE_QUEEN:
            if new_nbrs:
                print 'Ich bin die Koenigin der welt!'

        # end of the FSM
        bump_beh_out = beh.bump_beh(MOTION_TV)
        if not state == STATE_QUEEN:
            beh_out = beh.subsume([beh_out, bump_beh_out])

        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        hba.set_msg(state, 0, 0)
Exemple #13
0
def winter():
    beh.init(0.22, 40, 0.5, 0.1)

    state = STATE_IDLE

    manual_control = False

    while True:
        # run the system updates
        new_nbrs = beh.update()

        nbr_list = neighbors.get_neighbors()

        beh_out = beh.BEH_INACTIVE

        # 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'):
                ##### This is one way to find a cutoff for being in light.
                ##### Make sure you press the 'r' button when the robot is
                ##### in the light!
                global BRIGHTNESS_THRESHOLDS
                for sensor_dir in BRIGHTNESS_THRESHOLDS.keys():
                    BRIGHTNESS_THRESHOLDS[
                        sensor_dir] = 0.85 * rone.light_sensor_get_value(
                            sensor_dir)
                #####
                initial_time = sys.time()
                state = STATE_LIGHT

        elif state == STATE_LIGHT:
            leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
            if manual_control:
                leds.set_pattern('gr', 'group', LED_BRIGHTNESS)
            nbr_in_dark = get_nearest_nbr_in_dark(nbr_list)
            if nbr_in_dark != None:
                bearing = neighbors.get_nbr_bearing(nbr_in_dark)
                bearing = bearing - math.pi
                bearing = math2.normalize_angle(bearing)
                beh_out = move_in_dir(bearing)

            if not manual_control:
                if not self_in_light():
                    dark_start_time = sys.time()
                    state = STATE_DARK

            if rone.button_get_value('b'):
                manual_control = True
                dark_start_time = sys.time()
                state = STATE_DARK
            elif rone.button_get_value('r'):
                manual_control = False
                state = STATE_IDLE

        elif state == STATE_DARK:
            leds.set_pattern('b', 'circle', LED_BRIGHTNESS)
            if manual_control:
                leds.set_pattern('br', 'group', LED_BRIGHTNESS)
            nbrs_in_light = get_nbrs_in_light()
            nbrs_in_dark = get_nbrs_in_dark()
            if len(nbrs_in_light) > 0:
                bearing = get_avg_bearing_to_nbrs(nbrs_in_light)
                beh_out = move_in_dir(bearing)
            elif len(nbrs_in_dark) > 0:
                bearing = get_avg_bearing_to_nbrs(nbrs_in_dark)
                beh_out = move_in_dir(bearing)

            if not manual_control:
                if self_in_light():
                    state = STATE_LIGHT
                elif sys.time() - dark_start_time > LIFESPAN:
                    score_time = hba.winter_time_keeper(initial_time)
                    hba.winter_score_calc(score_time, LED_BRIGHTNESS)
                    state = STATE_DEAD
            if rone.button_get_value('g'):
                manual_control = True
                state = STATE_LIGHT
            elif rone.button_get_value('r'):
                manual_control = False
                state = STATE_IDLE

        elif state == STATE_DEAD:
            pass

        # end of the FSM

##        bump_beh_out = beh.bump_beh(MOTION_TV)
##        beh_out = beh.subsume([beh_out, bump_beh_out])

# set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        msg = [0, 0, 0]
        msg[MSG_IDX_STATE] = state
        hba.set_msg(msg[0], msg[1], msg[2])
Exemple #14
0
def message_demo():
    beh.init(0.22, 40, 0.5, 0.1)

    state = STATE_LEADER

    num_r = 1
    num_g = 1
    num_b = 1
    button_red_old = 0
    button_green_old = 0
    button_blue_old = 0

    while True:
        # run the system updates
        new_nbrs = beh.update()
        nbr_list = neighbors.get_neighbors()

        # init the velocities
        beh_out = beh.BEH_INACTIVE

        # read the buttons and update the local counts
        (num_r, button_red_old) = check_button("r", num_r, button_red_old)
        (num_g, button_green_old) = check_button("g", num_g, button_green_old)
        (num_b, button_blue_old) = check_button("b", num_b, button_blue_old)

        # build this robot's neighbor message
        distance_local = num_r
        mode_local = num_g
        quality_local = num_b
        hba.set_msg(distance_local, mode_local, quality_local)

        # find the neighbor with the lowest ID
        nbr_leader = nbrList_getRobotWithLowID(nbr_list)

        # are you the leader?
        if nbr_leader == None:
            # no neighbors.  you are the leader
            state = STATE_LEADER
        else:
            low_ID = neighbors.get_nbr_id(nbr_leader)
            #            if new_nbrs:
            #                print 'nbr lowID=',nbr_leader
            if low_ID < rone.get_id():
                state = STATE_MINION
            else:
                state = STATE_LEADER

        # this is the main finite-state machine
        if state == STATE_LEADER:
            msg = (distance_local, mode_local, quality_local)
            if new_nbrs:
                print "leader:", msg
            brightness = LED_BRIGHTNESSER

        elif state == STATE_MINION:
            if nbr_leader != None:
                msg = hba.get_msg_from_nbr(nbr_leader, new_nbrs)
                if new_nbrs:
                    print "minion. leader = ", str(low_ID), " msg=", msg
            brightness = LED_BRIGHTNESS

        # end of the FSM
        # unpack the message and display it on the LEDs
        (distance, mode, quality) = msg
        leds.set_pattern((distance, mode, quality), "count", brightness)

        # set the velocities
        beh.motion_set(beh_out)
Exemple #15
0
def fall(): 
    found_flower = False
    start_time = 0
    target_theta = 0
    my_color = -1
    beh.init(0.22, 40, 0.5, 0.1)
    state = STATE_IDLE
    color = 'r' #for flowers only

    def wander():
        state = STATE_WANDER
    def collect_pollen():
        state = STATE_COLLECT_POLLEN
        start_time = sys.time()
    def align_with(target):
        target_theta = target
        pose.set_pose(0,0,0)
        state = STATE_ALIGN
        start_time = sys.time()

    #motion_start_odo = pose.get_odometer()

    while True:
        beh.init(0.22, 40, 0.5, 0.1)

        new_nbrs = beh.update()
        nbrList = neighbors.get_neighbors()       
        if new_nbrs:
            print nbrList
        beh_out = beh.BEH_INACTIVE

        #FINITE STATE MACHINE
        if state == STATE_IDLE:
            leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_MOVE_TO_FLOWER
            if rone.button_get_value('b'):
                state = STATE_QUEEN
            if new_nbrs:
                print "idle"

        elif state == STATE_WANDER: #run forward, avoid direction of neighbors
            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            if nav_tower == None:
                state = STATE_RETURN_TO_BASE
            else:
                beh_out = beh.avoid_nbr(nav_tower, MOTION_TV)

                (flower, color) = detflower(nbrList)
                if flower != None:
                    state = STATE_MOVE_TO_FLOWER

        elif state == STATE_MOVE_TO_FLOWER:
            leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
            (flower, color) = detflower(nbrList)
            if flower != None:
                if (neighbors.get_nbr_range_bits(flower) > 6) or (beh.bump_angle_get() != None):
                    collect_pollen() #collect pollen if we bump or get close
                else:
                #otherwise keep following that flower
                    beh_out = beh.follow_nbr(flower, MOTION_TV)

        elif state == STATE_COLLECT_POLLEN:
            motion_start_odo = pose.get_odometer()
            if sys.time() > (collect_pollen_start_time + COLLECT_POLLEN_TIME):
                state = STATE_RETURN_TO_BASE
                found_flower = True
            elif sys.time() < (collect_pollen_start_time + BACK_UP_TIME):    
                tv = -MOTION_TV
                rv = 0
                beh_out = beh.tvrv(tv,rv) 
                turn_start_time = (collect_pollen_start_time + BACK_UP_TIME)

            elif sys.time() < (turn_start_time + TURN_TIME): 
                tv = 40
                rv = -MOTION_RV
                beh_out = beh.tvrv(tv,rv)

            else: 
                tv = MOTION_TV
                rv = (MOTION_RV - 300)
                beh_out = beh.tvrv(tv,rv)

        elif state == STATE_RETURN_TO_BASE:
            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            queen = find_queen(nbrList)
            if (nav_tower == None) and (queen == None):
                beh_out = (-MOTION_TV, 0, True)
            elif (nav_tower != None) and (queen == None):
                beh_out = beh.follow_nbr(nav_tower)
            elif neighbors.get_nbr_range_bits(queen) > 2:
                beh_out = beh.follow_nbr(queen, MOTION_TV)
            elif found_flower:
                state = STATE_RECRUIT
                start_time = sys.time()
            else:
                state = STATE_FOLLOW
                start_time = sys.time()

        elif state == STATE_FOLLOW:
            recruiter = find_recruiter()
            if recruiter == None:
                beh_out = beh.BEH_INACTIVE
                if sys.time() > (follow_start_time + FOLLOW_TIME):
                    wander()
            else:
                bearing = neighbors.get_nbr_bearing(recruiter)
                orientation = neighbors.get_nbr_orientation(recruiter)
                align_with(math.pi + bearing - orientation)

        elif state == STATE_GO:
            flower = detflower()
            if not flower == None:
                state = STATE_MOVE_TO_FLOWER
            beh_out = beh.tvrv(MOTION_TV, 0)

        elif state == STATE_RECRUIT:
            if sys.time() > (recruit_start_time + RECRUIT_TIME):
                align_with(pose.get_theta() - math.pi)

        elif state == STATE_ALIGN:
            tv = 0
            heading_error = math.normalize_angle(pose.get_theta() - target_theta)
            rv = ROTATE_RV_GAIN * heading_error
            beh_out = beh.tvrv(tv, rv)
            # you could actually do a running average in the list here
            small_error = hba.average_error_check(heading_error, [], HEADING_ERROR_LIMIT, new_nbrs)
            if new_nbrs:
                print "error", error_list
            if small_error:
                state = STATE_GO
        #END OF FINITE STATE MACHINE 

        bump_beh_out = beh.bump_beh(MOTION_TV)
        if state not in [STATE_RETURN_TO_BASE, STATE_COLLECT_POLLEN, STATE_RECRUIT]:
            beh_out = beh.subsume([beh_out, bump_beh_out])
        beh.motion_set(beh_out)
        hba.set_msg(state, my_color, 0)
Exemple #16
0
def spring():
    at_tree_odo = None
    tree_pose = None
    followers = 0
    have_seen_leader = False
    beh.init(0.22, 40, 0.5, 0.1)
    motion.init_rv(1000, MOTION_RV, MOTION_CAPTURE_DISTANCE,
                   MOTION_RELEASE_DISTANCE, MOTION_CAPTURE_ANGLE,
                   MOTION_RELEASE_ANGLE)
    state = STATE_IDLE

    while True:
        # run the system updates
        new_nbrs = beh.update()
        nbr_list = neighbors.get_neighbors()
        if new_nbrs:
            print state

        beh_out = beh.BEH_INACTIVE

        # set colors, because why not do it at the top
        color_counts = [0, 0, 0]
        for i in range(3):
            color_counts[i] = min([5, (state - 5 * i)])
        if state == STATE_IDLE:
            leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
        elif state == STATE_SUCCESS:
            leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
        else:
            leds.set_pattern(color_counts, 'count', LED_BRIGHTNESS)

        if rone.button_get_value('g'):
            tree_pose = None
            followers = 0
            have_seen_leader = False
            state = STATE_IDLE

        # this is the main finite-state machine
        if not state in [
                STATE_IDLE, STATE_LEADER, STATE_SUCCESS, STATE_FOLLOW
        ]:
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state in [STATE_LEADER, STATE_SUCCESS]:
                    start_time = sys.time()
                    state = STATE_FOLLOW

        if state == STATE_IDLE:

            if rone.button_get_value('r'):
                pose.set_pose(0, 0, 0)
                state = STATE_WANDER
            elif rone.button_get_value('b'):
                state = STATE_QUEEN

        elif state == STATE_QUEEN:
            pass

        elif state == STATE_WANDER:
            ##            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            nav = hba.find_nav_tower_nbr(NAV_ID)
            beh_out = beh.avoid_nbr(nav, MOTION_TV)
            queen = None
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_QUEEN:
                    queen = nbr

            if bump_front():
                if queen != None:
                    state = STATE_SUCCESS
                else:
                    tree_pose = pose.get_pose()
                    motion.set_goal((0.0, 0.0), MOTION_TV)
                    at_tree_odo = pose.get_odometer()
                    state = STATE_RETURN
            elif nav == None:
                motion.set_goal((0.0, 0.0), MOTION_TV)
                state = STATE_RETURN

        elif state == STATE_RETURN:
            ##            nav_tower = hba.find_nav_tower_nbr(NAV_ID)
            queen = None
            recruiter = None
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_QUEEN:
                    queen = nbr
                elif nbr_state == STATE_RECRUIT:
                    recruiter = nbr
            if queen != None:
                if (recruiter == None) and (tree_pose != None) and \
                close_to_nbr(queen):
                    start_time = sys.time()
                    dist_traveled = pose.get_odometer() - at_tree_odo
                    at_queen_odo = pose.get_odometer()
                    state = STATE_RECRUIT
                elif not closer_to_nbr(queen):
                    beh_out = beh.follow_nbr(queen, MOTION_TV)
                else:
                    start_time = sys.time()
                    state = STATE_FOLLOW
            else:
                (tv, rv) = motion.update()
                beh_out = beh.tvrv(tv, rv)

        elif state == STATE_RECRUIT:
            new_followers = 0
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_RECRUIT:
                    if neighbors.get_nbr_id(nbr) > rone.get_id():
                        state = STATE_FOLLOW
                elif nbr_state in [STATE_FOLLOW, STATE_QUEEN]:
                    new_followers += 1
            if new_followers > followers:
                print 'reset timer'
                start_time = sys.time()
            followers = max([followers, new_followers])
            if followers == 4 or sys.time() > start_time + WAIT_TIME:
                tree_pos = (tree_pose[0], tree_pose[1])
                motion.set_goal(tree_pos, MOTION_TV)
                state = STATE_LEADER

        elif state == STATE_FOLLOW:
            recruiter = None
            leader = None
            success = False
            new_followers = 1
            for nbr in nbr_list:
                nbr_state = hba.get_msg_from_nbr(nbr, new_nbrs)[MSG_IDX_STATE]
                if nbr_state == STATE_RECRUIT:
                    recruiter = nbr
                elif nbr_state == STATE_LEADER:
                    leader = nbr
                elif nbr_state == STATE_SUCCESS:
                    leader = nbr
                    success = True
                elif nbr_state in [STATE_FOLLOW, STATE_WANDER]:
                    new_followers += 1
            if success:
                have_seen_leader = True
            if new_followers > followers:
                start_time = sys.time()
            followers = max([followers, new_followers])

            if recruiter == None:
                if leader == None:
                    if have_seen_leader:
                        beh_out = beh.tvrv(MOTION_TV, 0)
                    elif followers == 5 or sys.time() > start_time + WAIT_TIME:
                        followers = 0
                        state = STATE_WANDER
                else:
                    if success and bump() and close_to_nbr(leader):
                        state = STATE_SUCCESS
                    beh_out = beh.follow_nbr(leader, MOTION_TV)

        elif state == STATE_LEADER:
            ##            (tv, rv) = motion.update()
            ##            beh_out = beh.tvrv(tv, rv)
            ##            if motion.is_done():
            ##                state = STATE_SUCCESS
            beh_out = beh.tvrv(-MOTION_TV, 0)

            if bump() or (pose.get_odometer() - at_queen_odo) > dist_traveled:
                state = STATE_SUCCESS

        elif state == STATE_SUCCESS:
            pass

        # end of the FSM

        if state not in [
                STATE_IDLE, STATE_RECRUIT, STATE_LEADER, STATE_SUCCESS,
                STATE_QUEEN
        ]:
            bump_beh_out = beh.bump_beh(MOTION_TV)
            beh_out = beh.subsume([beh_out, bump_beh_out])

        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        msg = [0, 0, 0]
        msg[MSG_IDX_STATE] = state
        hba.set_msg(msg[0], msg[1], msg[2])
Exemple #17
0
def recruit_demo():
    beh.init(0.22, 40, 0.5, 0.1)

    state = STATE_IDLE

    while True:
        # run the system updates
        new_nbrs = beh.update()

        nbr_list = neighbors.get_neighbors()
        beh_out = beh.BEH_INACTIVE

        # this is the main finite-state machine
        if state == STATE_IDLE:
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
            if rone.button_get_value('r'):
                state = STATE_MATCH_HEADING
                error_list = []
            if new_nbrs:
                print "idle"
                #print nbr_list

        elif state == STATE_MATCH_HEADING:
            leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
            if new_nbrs:
                print "match heading"

            # match the heading with the first robot on your nbr list
            # use the average_error_check() function to wait until you get a accurate match
            # for this demo, assume the first robot on the list is a dancing_nbr that is recruiting you to a flower
            dancing_nbr = nbrList_getFirstRobot(nbr_list)
            if dancing_nbr != None:
                # rotate to face the "dancing" robot
                tv = 0
                (rv, heading_error) = match_nbr_heading(dancing_nbr)
                beh_out = beh.tvrv(tv, rv)
                small_error = hba.average_error_check(heading_error,
                                                      error_list,
                                                      HEADING_ERROR_LIMIT,
                                                      new_nbrs)
                if new_nbrs:
                    print "error", error_list
                if small_error:
                    # We have a good heading match.  Go get pollen!
                    state = STATE_MOVE_TO_FLOWER
                    collect_pollen_start_odo = pose.get_odometer()

        elif state == STATE_MOVE_TO_FLOWER:
            # move forward
            beh_out = beh.tvrv(MOTION_TV, 0)

            # stop after a fixed distance_to_go
            distance_to_go = (collect_pollen_start_odo +
                              MOVE_TO_FLOWER_DISTANCE) - pose.get_odometer()
            if distance_to_go < 0:
                state = STATE_IDLE

            if new_nbrs:
                print "move to flower dist:", distance_to_go
            leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)

        # end of the FSM

        # set the beh velocities
        beh.motion_set(beh_out)

        #set the HBA message
        hba.set_msg(0, 0, 0)