def leaderElection_ID(list, includeSelf): leader_id = 0 leader = 0 if list and len(list) > 0: for nbr in list: nbr_id = neighbors.get_nbr_id(nbr) if (nbr_id > leader_id): leader = nbr leader_id = temp_id if includeSelf: if rone.get_id() > leader_id: return (0, rone.get_id()) else: return (leader, leader_id)
def FTL_sorted(): tv = 0 rv = 0 nbr_list = neighborsX.get_neighbors() if len(nbr_list) > 0: # You have neighbors. Follow max-min robot-id. If you're the lowest, become leader. follow_id = -1 follow_nbr = None for nbr in nbr_list: nbr_id = neighborsX.get_nbr_id(nbr) if nbr_id < rone.get_id() and nbr_id > follow_id: follow_id = nbr_id follow_nbr = nbr if follow_id == -1: # leader leds.set_pattern('rg', 'blink_fast', LED_BRIGHTNESS) tv = FTL_TV rv = 0 #bump_avoid() else: # follower leds.set_pattern('b', 'blink_fast', LED_BRIGHTNESS) (tv, rv) = follow_motion_controller(follow_nbr) else: # no neighbors. do nothing leds.set_pattern('r', 'circle', LED_BRIGHTNESS) velocity.set_tvrv(tv, rv)
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])
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)
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])
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)
def update(): current_time = sys.time() if current_time < _nbr_state['time_ir_xmit']: # not time yet to update the _nbr_state return False _nbr_state['time_ir_xmit'] += _nbr_state['nbr_period'] while _nbr_state['time_ir_xmit'] < current_time: _nbr_state['time_ir_xmit'] += _nbr_state['nbr_period'] # transmit your announce message if _nbr_state['xmit_enable']: #The IR message only contains the robot ID rone.ir_comms_send_message() # add a '@' to the front of the radio message to mark it as a nbr message rone.radio_send_message('@' + chr(rone.get_id()) + _nbr_state['message']) # walk over neighbor list and timeout old neighbors nbr_list = _nbr_state['nbr_list'] nbr_idx = 0 while nbr_idx < len(nbr_list): if current_time > (nbr_get_update_time(nbr_list[nbr_idx]) + _nbr_state['nbr_timeout']): nbr_list.pop(nbr_idx) else: nbr_idx += 1 # time out old obstacles if current_time > (_nbr_state['obstacles_time'] + _nbr_state['nbr_timeout']): _nbr_state['obstacles'] = None # process new messages and update current neighbors while True: ir_msg = rone.ir_comms_get_message() if ir_msg == None: break (nbr_ID, nbr_bearing, nbr_orientation, nbr_range) = _process_nbr_message(ir_msg) #print 'msg recv', nbr_ID if nbr_ID == rone.get_id(): # this is your own message. Don't make a neighbor, but process it for obstacles (nbr_id, receivers_list, transmitters_list, nbr_range) = ir_msg _nbr_state['obstacles'] = receivers_list _nbr_state['obstacles_time'] = current_time continue # this message is from a neighbor. look for a previous message from this neighbor new_nbr = True nbr_idx = 0 while nbr_idx < len(nbr_list): if get_nbr_id(nbr_list[nbr_idx]) == nbr_ID: new_nbr = False #print 'update nbr ', nbr_ID break else: nbr_idx += 1 # Add or replace the nbr on the nbr list # note: the order of this tuple is important. It needs to match the getters above if new_nbr: nbr = (nbr_ID, '', nbr_bearing, nbr_orientation, nbr_range, current_time) nbr_list.append(nbr) else: nbr_msg = get_nbr_message(nbr_list[nbr_idx]) nbr = (nbr_ID, nbr_msg, nbr_bearing, nbr_orientation, nbr_range, current_time) nbr_list[nbr_idx] = nbr #print 'obs',_nbr_state['obstacles'] # update the radio messages every time, in case they arrive out-of-sync with the IR while True: #Look for neighbor radio messages on the radio queue and if the msg ID is there, make that robot's #msg the incoming message #TODO check for '@' char, push other messages back on gueue radio_msg = rone.radio_get_message_nbr() if radio_msg == None: # There are no more radio messages, finished updates break else: # radio_msg[0] = '@', radio_msg[1] = robot ID, radio_msg[2] is the message radio_msg_id = ord(radio_msg[1]) nbr_idx = 0 while nbr_idx < len(nbr_list): if get_nbr_id(nbr_list[nbr_idx]) == radio_msg_id: #make the radio message the nbr's message radio_msg = radio_msg[2:-1] #print '>',radio_msg,'<' (nbr_ID, old_msg, nbr_bearing, nbr_orientation, nbr_range, current_time) = nbr_list[nbr_idx] nbr = (nbr_ID, radio_msg, nbr_bearing, nbr_orientation, nbr_range, current_time) nbr_list[nbr_idx] = nbr break nbr_idx += 1 return True
def update(): current_time = sys.time() if current_time < _nbr_state['time_ir_xmit']: # not time yet to update the _nbr_state return False _nbr_state['time_ir_xmit'] += _nbr_state['nbr_period'] while _nbr_state['time_ir_xmit'] < current_time: _nbr_state['time_ir_xmit'] += _nbr_state['nbr_period'] # transmit your announce message if _nbr_state['xmit_enable']: #IR_msg = chr(rone.get_id()) + _nbr_state['message'] #rone.ir_comms_send_message(IR_msg) rone.ir_comms_send_message() rone.radio_send_message(chr(rone.get_id()) + _nbr_state['message']) # walk over neighbor list and timeout old neighbors nbr_list = _nbr_state['nbr_list'] nbr_idx = 0 while nbr_idx < len(nbr_list): if current_time > (nbr_get_update_time(nbr_list[nbr_idx]) + _nbr_state['nbr_timeout']): nbr_list.pop(nbr_idx) else: nbr_idx += 1 # time out old obstacles if current_time > (_nbr_state['obstacles_time'] + _nbr_state['nbr_timeout']): _nbr_state['obstacles'] = None # process new messages and update current neighbors while True: ir_msg = rone.ir_comms_get_message() if ir_msg == None: break (nbr_ID, nbr_bearing, nbr_orientation, nbr_range_bits) = _process_nbr_message(ir_msg) #print 'msg recv', nbr_ID if nbr_ID == rone.get_id(): # this is your own message. Don't make a neighbor, but process it for obstacles (nbr_id, receivers_list, transmitters_list, nbr_range_bits) = ir_msg _nbr_state['obstacles'] = receivers_list _nbr_state['obstacles_time'] = current_time continue # this message is from a neighbor. look for a previous message from this neighbor new_nbr = True nbr_idx = 0 while nbr_idx < len(nbr_list): if get_nbr_id(nbr_list[nbr_idx]) == nbr_ID: new_nbr = False #print 'update nbr ', nbr_ID break else: nbr_idx += 1 # Add or replace the nbr on the nbr list # note: the order of this tuple is important. It needs to match the getters above if new_nbr: nbr = (nbr_ID, '', nbr_bearing, nbr_orientation, nbr_range_bits, current_time) nbr_list.append(nbr) else: nbr_msg = get_nbr_message(nbr_list[nbr_idx]) nbr = (nbr_ID, nbr_msg, nbr_bearing, nbr_orientation, nbr_range_bits, current_time) nbr_list[nbr_idx] = nbr while True: #Look for neighbor on the radio queue and if the msg ID is there, make that robot's #msg the incoming message radio_msg = rone.radio_get_message() if radio_msg == None: # There are no more radio messages, finished updates break else: radio_msg_id = ord(radio_msg[0]) nbr_idx = 0 while nbr_idx < len(nbr_list): if get_nbr_id(nbr_list[nbr_idx]) == radio_msg_id: #make the radio message the nbr's message radio_msg = radio_msg[1:-1] #print '>',radio_msg,'<' (nbr_ID, old_msg, nbr_bearing, nbr_orientation, nbr_range_bits, current_time) = nbr_list[nbr_idx] nbr = (nbr_ID, radio_msg, nbr_bearing, nbr_orientation, nbr_range_bits, current_time) nbr_list[nbr_idx] = nbr break nbr_idx += 1 #print 'obs',_nbr_state['obstacles'] return True