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 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 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 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 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 motion_set(beh_val):
velocity.set_tvrv(get_tv(beh_val), get_rv(beh_val))
def motion_set(beh_val):
velocity.set_tvrv(get_tv(beh_val), get_rv(beh_val))
