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 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 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 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)
