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 follow_the_leader():
mode = MODE_IDLE
velocity.init(0.22, 40, 0.3, 0.1)
leds.init()
neighborsX.init(NEIGHBOR_PERIOD, compute_bearing, compute_orientation)
while mode == MODE_IDLE:
# First, wait for the user to press a button, then select states
# The user is already pressing a button. wait until they release
while check_buttons() != '':
leds_blink_all(LED_BRIGHTNESS)
# wait for a button press
while check_buttons() == '':
leds_blink_all(LED_BRIGHTNESS)
# finally, process the button information
buttons = check_buttons()
print buttons
if 'rg' == buttons:
mode = MODE_SORTED
elif 'bg' == buttons:
mode = MODE_FLOCK
elif 'rb' == buttons:
mode = MODE_ALIGN
elif 'r' in buttons:
mode = MODE_REMOTE
elif 'g' in buttons:
mode = MODE_LEADER
elif 'b' in buttons:
mode = MODE_FOLLOWER
# Now that you know your mode, run the main loop
while True:
# run the neighbor system
neighborsX.update()
# run the velocity controller
velocity.update()
# update the led animations
leds.update()
if mode == MODE_REMOTE:
FTL_remote()
elif mode == MODE_LEADER:
FTL_leader()
elif mode == MODE_FOLLOWER:
FTL_follower()
elif mode == MODE_SORTED:
FTL_sorted()
elif mode == MODE_FLOCK:
flock()
elif mode == MODE_ALIGN:
match_orientation()
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 init(kff, kff_offset, kp, ki):
velocity.init(kff, kff_offset, kp, ki)
leds.init()
pose.init()
motion.init()
neighbors.init(NBR_PERIOD)
def init(kff, kff_offset, kp, ki):
velocity.init(kff, kff_offset, kp, ki)
leds.init()
pose.init()
motion.init()
neighbors.init(NBR_PERIOD)