def FTL_remote(): buttons = check_buttons() if buttons != '': leds.set_pattern('r', 'blink_fast', LED_BRIGHTNESS) rone.radio_send_message(buttons) else: leds.set_pattern('r', 'circle', LED_BRIGHTNESS) #sleep for a bit to avoid continuous radio transmission sys.sleep(REMOTE_XMIT_DELAY)
def flock_demo(): mode = MODE_IDLE beh.init(0.22, 40, 0.5, 0.1) buttons = hba.wait_for_button() if 'r' in buttons: mode = MODE_REMOTE elif 'g' in buttons: mode = MODE_FLOCK # Now that you know your mode, run the main loop while True: # run the system updates new_nbrs = beh.update() beh_out = beh.BEH_INACTIVE if mode == MODE_REMOTE: buttons = hba.check_buttons() if buttons != '': leds.set_pattern('r','blink_fast',LED_BRIGHTNESS) rone.radio_send_message(buttons) else: leds.set_pattern('r','circle',LED_BRIGHTNESS) #sleep for a bit to avoid continuous radio transmission sys.sleep(REMOTE_XMIT_DELAY) elif mode == MODE_FLOCK: bump_beh_out = beh.bump_beh(FTL_TV) receive_beh_out = receive_beh() flock_beh_out = flock_beh() # if beh.get_active(bump_beh_out): # tv = beh.get_tv(bump_beh_out) # rv = beh.get_rv(bump_beh_out) # else: # tv = beh.get_tv(receive_beh_out) + beh.get_tv(flock_beh_out) # rv = beh.get_rv(receive_beh_out) + beh.get_rv(flock_beh_out) flock_beh_out = beh.sum(flock_beh_out, receive_beh_out) beh_out = beh.subsume([flock_beh_out, bump_beh_out]) # set the beh velocities beh.motion_set(beh_out) #set the HBA message hba.set_msg(0, 0, 0)
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