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
