def receive_beh(): global receive_msg_time, receive_msg_tv, receive_msg_rv radio_msg = rone.radio_get_message() active = True if radio_msg != None: # we have a message! put lights in active mode leds.set_pattern('g','blink_fast',LED_BRIGHTNESS) receive_msg_time = sys.time() (receive_msg_tv, receive_msg_rv) = remote_motion_controller(radio_msg) else: # no message. check for radio message timeout if sys.time() > (receive_msg_time + REMOTE_XMIT_DELAY * 3): # message timeout. stop the motors receive_msg_tv = 0 receive_msg_rv = 0 active = False leds.set_pattern('g','circle',LED_BRIGHTNESS) return (receive_msg_tv, receive_msg_rv, active)
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