Пример #1
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)
Пример #2
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)
Пример #3
0
Файл: fall.py Проект: va17/THBCP
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)
Пример #4
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)