def flock_beh(): # act on the information from the message. Note that this might be # information stored from the last message we received, because message # information remains active for a while nbr_list = neighbors.get_neighbors() if len(nbr_list) > 0: # any neighbor wil do. get the first neighbor x = 0.0 y = 0.0 for nbr in nbr_list: nbr_bearing = neighbors.get_nbr_bearing(nbr) nbr_orientation = neighbors.get_nbr_orientation(nbr) nbr_heading = math2.normalize_angle(math.pi + nbr_bearing - nbr_orientation) x += math.cos(nbr_heading) y += math.sin(nbr_heading) nbr_heading_avg = math.atan2(y, x) beh = (0, FLOCK_RV_GAIN * nbr_heading_avg, True) else: #no neighbors. do nothing beh = (0, 0, False) return beh
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 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 match_nbr_heading(nbr): nbr_brg = neighbors.get_nbr_bearing(nbr) nbr_ornt = neighbors.get_nbr_orientation(nbr) heading_error = math2.normalize_angle(math.pi + nbr_brg - nbr_ornt) rv = ROTATE_RV_GAIN * heading_error return (rv, heading_error)