def get_nearest_nbr_in_dark(nbr_list): nbrs_in_dark = get_nbrs_in_dark() nearest = None if len(nbrs_in_dark) > 0: nearest = nbrs_in_dark[0] for nbr in nbrs_in_dark: if neighbors.get_nbr_range_bits( nbr) > neighbors.get_nbr_range_bits(nearest): nearest = nbr ## if neighbors.get_nbr_range_bits(nearest) < CLOSENESS_CONSTANT: ## nearest = None return nearest
def closer_to_nbr(nbr): return neighbors.get_nbr_range_bits(nbr) >= RANGE_BITS_CLOSER
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)
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)
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)
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)