def flower():
beh.init(0.22, 40, 0.5, 0.1)
flower_type = TYPE_RED
color = 'r'
# Broadcast the ID with a message saying that it is a flower
while True:
# Do updates
newNbrs = beh.update()
beh_out = beh.BEH_INACTIVE
if rone.button_get_value('r'):
flower_type = TYPE_RED
color = 'r'
if rone.button_get_value('g'):
flower_type = TYPE_GREEN
color = 'g'
if rone.button_get_value('b'):
flower_type = TYPE_BLUE
color = 'b'
hba.set_msg(0, 0, flower_type)
leds.set_pattern(color, 'ramp_slow', LED_BRIGHTNESS)
beh.motion_set(beh_out)
def flower():
beh.init(0.22, 40, 0.5, 0.1)
flower_type = TYPE_RED;
color = 'r'
# Broadcast the ID with a message saying that it is a flower
while True:
# Do updates
newNbrs = beh.update()
beh_out = beh.BEH_INACTIVE
if rone.button_get_value('r'):
flower_type = TYPE_RED
color = 'r'
if rone.button_get_value('g'):
flower_type = TYPE_GREEN
color = 'g'
if rone.button_get_value('b'):
flower_type = TYPE_BLUE
color = 'b'
hba.set_msg(0, 0, flower_type)
leds.set_pattern(color, 'ramp_slow', LED_BRIGHTNESS)
beh.motion_set(beh_out)
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 average_orientation():
# calculate average heading of all neighbors
# compute vector sum of orientation angles and then average
nbr_list = neighborsX.get_neighbors()
if len(nbr_list) > 0:
# You have neighbors
vector_sum = [0, 0]
for nbr in nbr_list:
nbr_heading = neighbor_heading(nbr)
vector_sum[0] += math.cos(nbr_heading)
vector_sum[1] += math.sin(nbr_heading)
average_heading = math.atan2(vector_sum[1], vector_sum[0])
return (FTL_TV, bound(MOTION_RV_GAIN * average_heading, MOTION_RV_MAX))
else:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
return (0, 0)
def match_orientation():
# pair-wise orientation match from high-id to low-id
tv = 0
rv = 0
nbr_list = neighborsX.get_neighbors()
if len(nbr_list) > 0:
# You have neighbors. Follow any neighbor. get the first neighbor
leds.set_pattern('rb', 'blink_fast', LED_BRIGHTNESS)
nbr = nbr_list[0]
rv = bound(MOTION_RV_GAIN * neighbor_heading(nbr), MOTION_RV_MAX)
else:
#no neighbors. do nothing
leds.set_pattern('b', 'circle', LED_BRIGHTNESS)
# student code end
velocity.set_tvrv(tv, rv)
def FTL_follower():
# act on the information from the nbr state. Note that this might be
# information stored from the last message we received, because message
# information remains active for a while
tv = 0
rv = 0
nbr_list = neighborsX.get_neighbors()
if len(nbr_list) > 0:
# You have neighbors. Follow any neighbor. get the first neighbor
leds.set_pattern('b', 'blink_fast', LED_BRIGHTNESS)
nbr = nbr_list[0]
(tv, rv) = follow_motion_controller(nbr)
else:
#no neighbors. do nothing
leds.set_pattern('b', 'circle', LED_BRIGHTNESS)
velocity.set_tvrv(tv, rv)
def FTL_leader():
global radio_msg_time, FTL_leader_tv, FTL_leader_rv
radio_msg = radio_get_message()
if radio_msg != None:
# we have a message! put lights in active mode
leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
radio_msg_time = sys.time()
(FTL_leader_tv, FTL_leader_rv) = leader_motion_controller(radio_msg)
else:
# no message. check for radio message timeout
if sys.time() > (radio_msg_time + REMOTE_XMIT_DELAY * 3):
# message timeout. stop the motors
FTL_leader_tv = 0
FTL_leader_rv = 0
leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
velocity.set_tvrv(FTL_leader_tv, FTL_leader_rv)
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 nav_tower_motion():
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_MOVE_TO_TOWER
motion_start_odo = pose.get_odometer()
while True: #while True loop just for testing, will need to call many times
nav_tower = hba.find_nav_tower_nbr(127)
if state == STATE_MOVE_TO_TOWER:
new_nbrs = beh.update()
# Move towards the nav_tower until turning around distance reached
if nav_tower != None: # move forward
beh_out = beh.follow_nbr(nav_tower, MOTION_TV)
leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
else:
leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
beh_out = beh.follow_nbr(nav_tower, MOTION_TV)
distance_to_go = (motion_start_odo + MOVE_TO_TOWER_DISTANCE) - pose.get_odometer()
beh.motion_set(beh_out)
if distance_to_go < 0:
state = STATE_IDLE
return
def nav_tower_motion():
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_MOVE_TO_TOWER
motion_start_odo = pose.get_odometer()
while True: #while True loop just for testing, will need to call many times
nav_tower = hba.find_nav_tower_nbr(127)
if state == STATE_MOVE_TO_TOWER:
new_nbrs = beh.update()
# Move towards the nav_tower until turning around distance reached
if nav_tower != None: # move forward
beh_out = beh.follow_nbr(nav_tower, MOTION_TV)
leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
else:
leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
beh_out = beh.follow_nbr(nav_tower, MOTION_TV)
distance_to_go = (motion_start_odo +
MOVE_TO_TOWER_DISTANCE) - pose.get_odometer()
beh.motion_set(beh_out)
if distance_to_go < 0:
state = STATE_IDLE
return
def flock():
# have the robots follow the leader as a flock
global FTL_leader_tv, FTL_leader_rv
tv = 0
rv = 0
nbr_list = neighborsX.get_neighbors()
if len(nbr_list) > 0:
leds.set_pattern('bg', 'blink_fast', LED_BRIGHTNESS)
(tv, rv) = average_orientation()
radio_msg = radio_get_message()
if radio_msg != None:
# we have a message! put lights in active mode
(FTL_leader_tv,
FTL_leader_rv) = leader_motion_controller(radio_msg)
rv += FTL_leader_rv
else:
# no neighbors. do nothing
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
velocity.set_tvrv(tv, rv)
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 winter_score_calc(score_time, brightness):
score = score_time/1000
interval_1 = 300
interval_2 = 180
interval_3 = 600-interval_1-interval_2
if score<=interval_1:
score1=score/(interval_1/5)
score2=0
score3=0
elif score<=(interval_2+interval_1):
score1=5
score2=(score-interval_1)/(interval_2/5)
score3=0
elif score<=(interval_3+interval_2+interval_1):
score1=5
score2=5
score3=(score-interval_1-interval_2)/(interval_3/5)
else:
score1=5
score2=5
score3=5
print score,score1,score2,score3
leds.set_pattern((int(score1), int(score2), int(score3)), 'count', brightness)
def wall_follow_demo():
velocity.init(0.22, 40, 0.5, 0.1)
leds.init()
pose.init()
motion.init()
neighbors.init(NBR_PERIOD)
state = STATE_IDLE
wall_time = 0
while True:
# Do updates
leds.update()
pose.update()
velocity.update()
new_nbrs = neighbors.update()
nbrList = neighbors.get_neighbors()
tv = 0
rv = 0
# this is the main finite-state machine
if state == STATE_IDLE:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
if new_nbrs:
print "idle"
if rone.button_get_value('r'):
state = STATE_LOOK_FOR_WALL
elif state == STATE_LOOK_FOR_WALL:
leds.set_pattern('r', 'blink_fast', LED_BRIGHTNESS)
if new_nbrs:
print "look for wall"
tv = MOTION_TV
obs = neighbors.get_obstacles()
if (obs != None):
state = STATE_WALL_FOLLOW
elif state == STATE_WALL_FOLLOW:
leds.set_pattern('b', 'blink_fast', LED_BRIGHTNESS)
if new_nbrs:
print "wall follow"
# follow the wall
(tv, rv, active) = wall_follow(MOTION_TV / 2)
if active == True:
wall_time = sys.time()
if sys.time() > (wall_time + WALL_TIMEOUT):
state = STATE_LOOK_FOR_WALL
# end of the FSM
# set the velocities
velocity.set_tvrv(tv, rv)
#set the message
hba.set_msg(0, 0, 0)
def winter_score_calc(score_time, brightness):
score = score_time / 1000
interval_1 = 300
interval_2 = 180
interval_3 = 600 - interval_1 - interval_2
if score <= interval_1:
score1 = score / (interval_1 / 5)
score2 = 0
score3 = 0
elif score <= (interval_2 + interval_1):
score1 = 5
score2 = (score - interval_1) / (interval_2 / 5)
score3 = 0
elif score <= (interval_3 + interval_2 + interval_1):
score1 = 5
score2 = 5
score3 = (score - interval_1 - interval_2) / (interval_3 / 5)
else:
score1 = 5
score2 = 5
score3 = 5
print score, score1, score2, score3
leds.set_pattern((int(score1), int(score2), int(score3)), 'count',
brightness)
def fall():
found_flower = False
start_time = 0
target_theta = 0
my_color = -1
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_IDLE
color = 'r' #for flowers only
def wander():
state = STATE_WANDER
def collect_pollen():
state = STATE_COLLECT_POLLEN
start_time = sys.time()
def align_with(target):
target_theta = target
pose.set_pose(0,0,0)
state = STATE_ALIGN
start_time = sys.time()
#motion_start_odo = pose.get_odometer()
while True:
beh.init(0.22, 40, 0.5, 0.1)
new_nbrs = beh.update()
nbrList = neighbors.get_neighbors()
if new_nbrs:
print nbrList
beh_out = beh.BEH_INACTIVE
#FINITE STATE MACHINE
if state == STATE_IDLE:
leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_MOVE_TO_FLOWER
if rone.button_get_value('b'):
state = STATE_QUEEN
if new_nbrs:
print "idle"
elif state == STATE_WANDER: #run forward, avoid direction of neighbors
nav_tower = hba.find_nav_tower_nbr(NAV_ID)
if nav_tower == None:
state = STATE_RETURN_TO_BASE
else:
beh_out = beh.avoid_nbr(nav_tower, MOTION_TV)
(flower, color) = detflower(nbrList)
if flower != None:
state = STATE_MOVE_TO_FLOWER
elif state == STATE_MOVE_TO_FLOWER:
leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
(flower, color) = detflower(nbrList)
if flower != None:
if (neighbors.get_nbr_range_bits(flower) > 6) or (beh.bump_angle_get() != None):
collect_pollen() #collect pollen if we bump or get close
else:
#otherwise keep following that flower
beh_out = beh.follow_nbr(flower, MOTION_TV)
elif state == STATE_COLLECT_POLLEN:
motion_start_odo = pose.get_odometer()
if sys.time() > (collect_pollen_start_time + COLLECT_POLLEN_TIME):
state = STATE_RETURN_TO_BASE
found_flower = True
elif sys.time() < (collect_pollen_start_time + BACK_UP_TIME):
tv = -MOTION_TV
rv = 0
beh_out = beh.tvrv(tv,rv)
turn_start_time = (collect_pollen_start_time + BACK_UP_TIME)
elif sys.time() < (turn_start_time + TURN_TIME):
tv = 40
rv = -MOTION_RV
beh_out = beh.tvrv(tv,rv)
else:
tv = MOTION_TV
rv = (MOTION_RV - 300)
beh_out = beh.tvrv(tv,rv)
elif state == STATE_RETURN_TO_BASE:
nav_tower = hba.find_nav_tower_nbr(NAV_ID)
queen = find_queen(nbrList)
if (nav_tower == None) and (queen == None):
beh_out = (-MOTION_TV, 0, True)
elif (nav_tower != None) and (queen == None):
beh_out = beh.follow_nbr(nav_tower)
elif neighbors.get_nbr_range_bits(queen) > 2:
beh_out = beh.follow_nbr(queen, MOTION_TV)
elif found_flower:
state = STATE_RECRUIT
start_time = sys.time()
else:
state = STATE_FOLLOW
start_time = sys.time()
elif state == STATE_FOLLOW:
recruiter = find_recruiter()
if recruiter == None:
beh_out = beh.BEH_INACTIVE
if sys.time() > (follow_start_time + FOLLOW_TIME):
wander()
else:
bearing = neighbors.get_nbr_bearing(recruiter)
orientation = neighbors.get_nbr_orientation(recruiter)
align_with(math.pi + bearing - orientation)
elif state == STATE_GO:
flower = detflower()
if not flower == None:
state = STATE_MOVE_TO_FLOWER
beh_out = beh.tvrv(MOTION_TV, 0)
elif state == STATE_RECRUIT:
if sys.time() > (recruit_start_time + RECRUIT_TIME):
align_with(pose.get_theta() - math.pi)
elif state == STATE_ALIGN:
tv = 0
heading_error = math.normalize_angle(pose.get_theta() - target_theta)
rv = ROTATE_RV_GAIN * heading_error
beh_out = beh.tvrv(tv, rv)
# you could actually do a running average in the list here
small_error = hba.average_error_check(heading_error, [], HEADING_ERROR_LIMIT, new_nbrs)
if new_nbrs:
print "error", error_list
if small_error:
state = STATE_GO
#END OF FINITE STATE MACHINE
bump_beh_out = beh.bump_beh(MOTION_TV)
if state not in [STATE_RETURN_TO_BASE, STATE_COLLECT_POLLEN, STATE_RECRUIT]:
beh_out = beh.subsume([beh_out, bump_beh_out])
beh.motion_set(beh_out)
hba.set_msg(state, my_color, 0)
def summer():
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_IDLE
while True:
# run the system updates
new_nbrs = beh.update()
nbrList = neighbors.get_neighbors()
if new_nbrs:
print nbrList
beh_out = beh.BEH_INACTIVE
# this is the main finite-state machine
if state == STATE_IDLE:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
leds.set_pattern('b', 'circle', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_FIND_QUEEN
if rone.button_get_value('b'):
state = STATE_QUEEN
if new_nbrs:
print "idle"
elif state == STATE_FIND_QUEEN:
leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
beh_out = beh.tvrv(MOTION_TV, 0)
queen = get_queen()
if not queen == None:
state = STATE_BUMP_QUEEN
else:
#go straight and hope for the best
beh_out = beh.tvrv(MOTION_TV, 0)
elif state == STATE_BUMP_QUEEN:
leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
queen = get_queen()
if queen == None:
state = STATE_RETURN
else:
if (neighbors.get_nbr_range_bits(queen) > 6) or (beh.bump_angle_get() != None):
state = STATE_BACK_UP
start_time = sys.time()
else:
beh_out = beh.follow_nbr(queen, MOTION_TV)
elif state == STATE_BACK_UP:
if sys.time() > start_time + BACK_UP_TIME:
state = STATE_RETURN
else:
beh_out = beh.tvrv(-MOTION_TV, 0)
elif state == STATE_RETURN:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_FIND_QUEEN
queen = get_queen()
if queen == None:
state = STATE_IDLE
else:
beh_out = beh.avoid_nbr(queen, MOTION_TV)
elif state == STATE_QUEEN:
if new_nbrs:
print 'Ich bin die Koenigin der welt!'
# end of the FSM
bump_beh_out = beh.bump_beh(MOTION_TV)
if not state == STATE_QUEEN:
beh_out = beh.subsume([beh_out, bump_beh_out])
# set the beh velocities
beh.motion_set(beh_out)
#set the HBA message
hba.set_msg(state, 0, 0)
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 summer():
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_IDLE
while True:
# run the system updates
new_nbrs = beh.update()
nbrList = neighbors.get_neighbors()
if new_nbrs:
print nbrList
beh_out = beh.BEH_INACTIVE
# this is the main finite-state machine
if state == STATE_IDLE:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
leds.set_pattern('b', 'circle', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_FIND_QUEEN
if rone.button_get_value('b'):
state = STATE_QUEEN
if new_nbrs:
print "idle"
elif state == STATE_FIND_QUEEN:
leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
beh_out = beh.tvrv(MOTION_TV, 0)
queen = get_queen()
if not queen == None:
state = STATE_BUMP_QUEEN
else:
#go straight and hope for the best
beh_out = beh.tvrv(MOTION_TV, 0)
elif state == STATE_BUMP_QUEEN:
leds.set_pattern('r', 'ramp_slow', LED_BRIGHTNESS)
queen = get_queen()
if queen == None:
state = STATE_RETURN
else:
if (neighbors.get_nbr_range_bits(queen) >
6) or (beh.bump_angle_get() != None):
state = STATE_BACK_UP
start_time = sys.time()
else:
beh_out = beh.follow_nbr(queen, MOTION_TV)
elif state == STATE_BACK_UP:
if sys.time() > start_time + BACK_UP_TIME:
state = STATE_RETURN
else:
beh_out = beh.tvrv(-MOTION_TV, 0)
elif state == STATE_RETURN:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_FIND_QUEEN
queen = get_queen()
if queen == None:
state = STATE_IDLE
else:
beh_out = beh.avoid_nbr(queen, MOTION_TV)
elif state == STATE_QUEEN:
if new_nbrs:
print 'Ich bin die Koenigin der welt!'
# end of the FSM
bump_beh_out = beh.bump_beh(MOTION_TV)
if not state == STATE_QUEEN:
beh_out = beh.subsume([beh_out, bump_beh_out])
# set the beh velocities
beh.motion_set(beh_out)
#set the HBA message
hba.set_msg(state, 0, 0)
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 waypoint_motion():
velocity.init(0.22, 40, 0.5, 0.1)
leds.init()
poseX.init(pose_update)
motionX.init(compute_goal_distance_and_heading, motion_controller_tv,
motion_controller_rv)
pose_estimator_print_time = sys.time()
mode = MODE_INACTIVE
pose_old = (0.0, 0.0, 0.0)
waypoint_list = []
while True:
# update the LED animations
leds.update()
# update the pose estimator
poseX.update()
# update the motion controller
(tv, rv) = motionX.update()
velocity.set_tvrv(tv, rv)
# update the velocity controller if you are active, otherwise coast so the robot can be pushed
if mode == MODE_ACTIVE:
velocity.update()
else:
rone.motor_set_pwm('l', 0)
rone.motor_set_pwm('r', 0)
# print status every 500ms
current_time = sys.time()
if sys.time() > pose_estimator_print_time:
pose_estimator_print_time += 250
print 'goal', motionX.get_goal(), 'pose', poseX.get_pose(
), 'odo', poseX.get_odometer()
if mode == MODE_INACTIVE:
if (math2.pose_subtract(poseX.get_pose(), pose_old) !=
(0.0, 0.0, 0.0)):
# We're moving! Yay! Blink excitedly!
leds.set_pattern('r', 'blink_fast',
int(LED_BRIGHTNESS * 1.5))
else:
# not moving. sad face.
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
pose_old = poseX.get_pose()
# check the buttons. If the red button is pressed, load the waypoint list
if rone.button_get_value('r'):
if mode == MODE_INACTIVE:
poseX.set_pose(0, 0, 0)
#waypoint_list = [(608, 0), (608, 304), (0, 304), (0, 0)]
waypoint_list = [(700, 0), (700, 700), (0, 700), (0, 0)]
mode = MODE_ACTIVE
# check to see if you are at your waypoint. If so, go to the next one
if mode == MODE_ACTIVE:
leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
if motionX.is_done():
## Do we have another waypoint?
if len(waypoint_list) > 0:
leds.set_pattern('rgb', 'group', LED_BRIGHTNESS)
sys.sleep(250)
waypoint = waypoint_list.pop(0)
print 'waypoint', waypoint
motionX.set_goal(waypoint, MOTION_TV)
else:
print 'waypoint list empty'
mode = MODE_INACTIVE
velocity.set_tvrv(0, 0)
def winter():
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_IDLE
manual_control = False
while True:
# run the system updates
new_nbrs = beh.update()
nbr_list = neighbors.get_neighbors()
beh_out = beh.BEH_INACTIVE
# this is the main finite-state machine
if state == STATE_IDLE:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
if new_nbrs:
print "idle"
if rone.button_get_value('r'):
##### This is one way to find a cutoff for being in light.
##### Make sure you press the 'r' button when the robot is
##### in the light!
global BRIGHTNESS_THRESHOLDS
for sensor_dir in BRIGHTNESS_THRESHOLDS.keys():
BRIGHTNESS_THRESHOLDS[
sensor_dir] = 0.85 * rone.light_sensor_get_value(
sensor_dir)
#####
initial_time = sys.time()
state = STATE_LIGHT
elif state == STATE_LIGHT:
leds.set_pattern('g', 'circle', LED_BRIGHTNESS)
if manual_control:
leds.set_pattern('gr', 'group', LED_BRIGHTNESS)
nbr_in_dark = get_nearest_nbr_in_dark(nbr_list)
if nbr_in_dark != None:
bearing = neighbors.get_nbr_bearing(nbr_in_dark)
bearing = bearing - math.pi
bearing = math2.normalize_angle(bearing)
beh_out = move_in_dir(bearing)
if not manual_control:
if not self_in_light():
dark_start_time = sys.time()
state = STATE_DARK
if rone.button_get_value('b'):
manual_control = True
dark_start_time = sys.time()
state = STATE_DARK
elif rone.button_get_value('r'):
manual_control = False
state = STATE_IDLE
elif state == STATE_DARK:
leds.set_pattern('b', 'circle', LED_BRIGHTNESS)
if manual_control:
leds.set_pattern('br', 'group', LED_BRIGHTNESS)
nbrs_in_light = get_nbrs_in_light()
nbrs_in_dark = get_nbrs_in_dark()
if len(nbrs_in_light) > 0:
bearing = get_avg_bearing_to_nbrs(nbrs_in_light)
beh_out = move_in_dir(bearing)
elif len(nbrs_in_dark) > 0:
bearing = get_avg_bearing_to_nbrs(nbrs_in_dark)
beh_out = move_in_dir(bearing)
if not manual_control:
if self_in_light():
state = STATE_LIGHT
elif sys.time() - dark_start_time > LIFESPAN:
score_time = hba.winter_time_keeper(initial_time)
hba.winter_score_calc(score_time, LED_BRIGHTNESS)
state = STATE_DEAD
if rone.button_get_value('g'):
manual_control = True
state = STATE_LIGHT
elif rone.button_get_value('r'):
manual_control = False
state = STATE_IDLE
elif state == STATE_DEAD:
pass
# end of the FSM
## 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 fall():
found_flower = False
start_time = 0
target_theta = 0
my_color = -1
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_IDLE
color = 'r' #for flowers only
def wander():
state = STATE_WANDER
def collect_pollen():
state = STATE_COLLECT_POLLEN
start_time = sys.time()
def align_with(target):
target_theta = target
pose.set_pose(0, 0, 0)
state = STATE_ALIGN
start_time = sys.time()
#motion_start_odo = pose.get_odometer()
while True:
beh.init(0.22, 40, 0.5, 0.1)
new_nbrs = beh.update()
nbrList = neighbors.get_neighbors()
if new_nbrs:
print nbrList
beh_out = beh.BEH_INACTIVE
#FINITE STATE MACHINE
if state == STATE_IDLE:
leds.set_pattern('rb', 'group', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_MOVE_TO_FLOWER
if rone.button_get_value('b'):
state = STATE_QUEEN
if new_nbrs:
print "idle"
elif state == STATE_WANDER: #run forward, avoid direction of neighbors
nav_tower = hba.find_nav_tower_nbr(NAV_ID)
if nav_tower == None:
state = STATE_RETURN_TO_BASE
else:
beh_out = beh.avoid_nbr(nav_tower, MOTION_TV)
(flower, color) = detflower(nbrList)
if flower != None:
state = STATE_MOVE_TO_FLOWER
elif state == STATE_MOVE_TO_FLOWER:
leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
(flower, color) = detflower(nbrList)
if flower != None:
if (neighbors.get_nbr_range_bits(flower) >
6) or (beh.bump_angle_get() != None):
collect_pollen() #collect pollen if we bump or get close
else:
#otherwise keep following that flower
beh_out = beh.follow_nbr(flower, MOTION_TV)
elif state == STATE_COLLECT_POLLEN:
motion_start_odo = pose.get_odometer()
if sys.time() > (collect_pollen_start_time + COLLECT_POLLEN_TIME):
state = STATE_RETURN_TO_BASE
found_flower = True
elif sys.time() < (collect_pollen_start_time + BACK_UP_TIME):
tv = -MOTION_TV
rv = 0
beh_out = beh.tvrv(tv, rv)
turn_start_time = (collect_pollen_start_time + BACK_UP_TIME)
elif sys.time() < (turn_start_time + TURN_TIME):
tv = 40
rv = -MOTION_RV
beh_out = beh.tvrv(tv, rv)
else:
tv = MOTION_TV
rv = (MOTION_RV - 300)
beh_out = beh.tvrv(tv, rv)
elif state == STATE_RETURN_TO_BASE:
nav_tower = hba.find_nav_tower_nbr(NAV_ID)
queen = find_queen(nbrList)
if (nav_tower == None) and (queen == None):
beh_out = (-MOTION_TV, 0, True)
elif (nav_tower != None) and (queen == None):
beh_out = beh.follow_nbr(nav_tower)
elif neighbors.get_nbr_range_bits(queen) > 2:
beh_out = beh.follow_nbr(queen, MOTION_TV)
elif found_flower:
state = STATE_RECRUIT
start_time = sys.time()
else:
state = STATE_FOLLOW
start_time = sys.time()
elif state == STATE_FOLLOW:
recruiter = find_recruiter()
if recruiter == None:
beh_out = beh.BEH_INACTIVE
if sys.time() > (follow_start_time + FOLLOW_TIME):
wander()
else:
bearing = neighbors.get_nbr_bearing(recruiter)
orientation = neighbors.get_nbr_orientation(recruiter)
align_with(math.pi + bearing - orientation)
elif state == STATE_GO:
flower = detflower()
if not flower == None:
state = STATE_MOVE_TO_FLOWER
beh_out = beh.tvrv(MOTION_TV, 0)
elif state == STATE_RECRUIT:
if sys.time() > (recruit_start_time + RECRUIT_TIME):
align_with(pose.get_theta() - math.pi)
elif state == STATE_ALIGN:
tv = 0
heading_error = math.normalize_angle(pose.get_theta() -
target_theta)
rv = ROTATE_RV_GAIN * heading_error
beh_out = beh.tvrv(tv, rv)
# you could actually do a running average in the list here
small_error = hba.average_error_check(heading_error, [],
HEADING_ERROR_LIMIT,
new_nbrs)
if new_nbrs:
print "error", error_list
if small_error:
state = STATE_GO
#END OF FINITE STATE MACHINE
bump_beh_out = beh.bump_beh(MOTION_TV)
if state not in [
STATE_RETURN_TO_BASE, STATE_COLLECT_POLLEN, STATE_RECRUIT
]:
beh_out = beh.subsume([beh_out, bump_beh_out])
beh.motion_set(beh_out)
hba.set_msg(state, my_color, 0)
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 flower_motion():
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_IDLE
while True:
# run the system updates
new_nbrs = beh.update()
nbrList = neighbors.get_neighbors()
if new_nbrs:
print nbrList
beh_out = beh.BEH_INACTIVE
# this is the main finite-state machine
if state == STATE_IDLE:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_MOVE_TO_FLOWER
if new_nbrs:
print "idle"
elif state == STATE_MOVE_TO_FLOWER:
leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
if new_nbrs:
print "move to flower"
# Move towards the flower until you bump into it
# for this demo, assume the first robot on the list is a flower
## flower = nbrList_getFirstRobot(nbrList)
(color,nbr) = detflower(nbrList)
flower = nbr
if flower != None:
# Stop if we get close or bump into the flower
#if neighbors.get_nbr_close_range(flower):
if (neighbors.get_nbr_range_bits(flower) > 6) or (beh.bump_angle_get() != None):
state = STATE_COLLECT_POLLEN
collect_pollen_start_time = sys.time()
else:
# Move to the flower
beh_out = beh.follow_nbr(flower, MOTION_TV)
#print beh_out
elif state == STATE_COLLECT_POLLEN:
# this is where you will put your clever pollen collection code
# we will just wait for a second, then leave. (this will not collect very much pollen)
leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
if new_nbrs:
print "collect"
# Timeout after 5 seconds
if sys.time() > (collect_pollen_start_time + COLLECT_POLLEN_TIME):
state = STATE_MOVE_AWAY_FLOWER
elif sys.time() < (collect_pollen_start_time + BACK_UP_TIME):
tv = -MOTION_TV
rv = 0
beh_out = beh.tvrv(tv,rv)
turn_start_time = (collect_pollen_start_time + BACK_UP_TIME)
elif sys.time() < (turn_start_time + TURN_TIME):
tv = 0
rv = -MOTION_RV
else:
tv = MOTION_TV
rv = MOTION_RV
beh_out = beh.tvrv(tv,rv)
elif state == STATE_MOVE_AWAY_FLOWER:
if new_nbrs:
print "avoid flower"
leds.set_pattern('r', 'blink_slow', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_MOVE_TO_FLOWER
# Move away from the flower until it is out of range
flower = nbrList_getFirstRobot(nbrList)
if flower != None:
# Point away the flower
beh_out = beh.avoid_nbr(flower, MOTION_TV)
else:
state = STATE_IDLE
# end of the FSM
bump_beh_out = beh.bump_beh(MOTION_TV)
if state != STATE_COLLECT_POLLEN:
beh_out = beh.subsume([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 recruit_demo():
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_IDLE
while True:
# run the system updates
new_nbrs = beh.update()
nbr_list = neighbors.get_neighbors()
beh_out = beh.BEH_INACTIVE
# this is the main finite-state machine
if state == STATE_IDLE:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_MATCH_HEADING
error_list = []
if new_nbrs:
print "idle"
#print nbr_list
elif state == STATE_MATCH_HEADING:
leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
if new_nbrs:
print "match heading"
# match the heading with the first robot on your nbr list
# use the average_error_check() function to wait until you get a accurate match
# for this demo, assume the first robot on the list is a dancing_nbr that is recruiting you to a flower
dancing_nbr = nbrList_getFirstRobot(nbr_list)
if dancing_nbr != None:
# rotate to face the "dancing" robot
tv = 0
(rv, heading_error) = match_nbr_heading(dancing_nbr)
beh_out = beh.tvrv(tv, rv)
small_error = hba.average_error_check(heading_error, error_list, HEADING_ERROR_LIMIT, new_nbrs)
if new_nbrs:
print "error", error_list
if small_error:
# We have a good heading match. Go get pollen!
state = STATE_MOVE_TO_FLOWER
collect_pollen_start_odo = pose.get_odometer()
elif state == STATE_MOVE_TO_FLOWER:
# move forward
beh_out = beh.tvrv(MOTION_TV, 0)
# stop after a fixed distance_to_go
distance_to_go = (collect_pollen_start_odo + MOVE_TO_FLOWER_DISTANCE) - pose.get_odometer()
if distance_to_go < 0:
state = STATE_IDLE
if new_nbrs:
print "move to flower dist:", distance_to_go
leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
# end of the FSM
# set the beh velocities
beh.motion_set(beh_out)
#set the HBA message
hba.set_msg(0, 0, 0)
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 recruit_demo():
beh.init(0.22, 40, 0.5, 0.1)
state = STATE_IDLE
while True:
# run the system updates
new_nbrs = beh.update()
nbr_list = neighbors.get_neighbors()
beh_out = beh.BEH_INACTIVE
# this is the main finite-state machine
if state == STATE_IDLE:
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
if rone.button_get_value('r'):
state = STATE_MATCH_HEADING
error_list = []
if new_nbrs:
print "idle"
#print nbr_list
elif state == STATE_MATCH_HEADING:
leds.set_pattern('b', 'ramp_slow', LED_BRIGHTNESS)
if new_nbrs:
print "match heading"
# match the heading with the first robot on your nbr list
# use the average_error_check() function to wait until you get a accurate match
# for this demo, assume the first robot on the list is a dancing_nbr that is recruiting you to a flower
dancing_nbr = nbrList_getFirstRobot(nbr_list)
if dancing_nbr != None:
# rotate to face the "dancing" robot
tv = 0
(rv, heading_error) = match_nbr_heading(dancing_nbr)
beh_out = beh.tvrv(tv, rv)
small_error = hba.average_error_check(heading_error,
error_list,
HEADING_ERROR_LIMIT,
new_nbrs)
if new_nbrs:
print "error", error_list
if small_error:
# We have a good heading match. Go get pollen!
state = STATE_MOVE_TO_FLOWER
collect_pollen_start_odo = pose.get_odometer()
elif state == STATE_MOVE_TO_FLOWER:
# move forward
beh_out = beh.tvrv(MOTION_TV, 0)
# stop after a fixed distance_to_go
distance_to_go = (collect_pollen_start_odo +
MOVE_TO_FLOWER_DISTANCE) - pose.get_odometer()
if distance_to_go < 0:
state = STATE_IDLE
if new_nbrs:
print "move to flower dist:", distance_to_go
leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
# end of the FSM
# set the beh velocities
beh.motion_set(beh_out)
#set the HBA message
hba.set_msg(0, 0, 0)