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
