def follow_nbr(nbr, tv): beh_out = BEH_INACTIVE if nbr != None: rv = neighbors.get_nbr_bearing(nbr) * tv * TVRV_RATIO * RV_FOLLOW_GAIN #tv = MOTION_TV beh_out = (tv, rv, True) return beh_out
def avoid_nbr(nbr, tv): beh_out = BEH_INACTIVE if nbr != None: bearing = math2.normalize_angle(neighbors.get_nbr_bearing(nbr) + math.pi) rv = bearing * tv * TVRV_RATIO * RV_FOLLOW_GAIN #tv = MOTION_TV beh_out = (tv, rv, True) return beh_out
def avoid_nbr(nbr, tv): beh_out = BEH_INACTIVE if nbr != None: bearing = math2.normalize_angle( neighbors.get_nbr_bearing(nbr) + math.pi) rv = bearing * tv * TVRV_RATIO * RV_FOLLOW_GAIN #tv = MOTION_TV beh_out = (tv, rv, True) return beh_out
def get_avg_bearing_to_nbrs(nbr_list): x = 0.0 y = 0.0 for nbr in nbr_list: bearing = neighbors.get_nbr_bearing(nbr) x += math.cos(bearing) y += math.sin(bearing) avg_bearing = math.atan2(y, x) avg_bearing = math2.normalize_angle(avg_bearing) return avg_bearing
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)
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])