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