def _process_nbr_message(ir_msg):
if ir_msg == None:
return None
else:
(nbr_id, receivers_list, transmitters_list, range_bits) = ir_msg
# compute the bearing from the receivers_list the message was received on
x = 0.0
y = 0.0
for r in receivers_list:
y += math.sin(math.pi/8 + r * math.pi/4)
x += math.cos(math.pi/8 + r * math.pi/4)
bearing = math2.normalize_angle(math.atan2(y, x))
# compute the orientation from the transmitters_list the message was received on
x = 0.0
y = 0.0
for t in transmitters_list:
y += math.sin(t * math.pi/4)
x += math.cos(t * math.pi/4)
orientation = math2.normalize_angle(math.atan2(y, x))
range_bits = len(receivers_list) + len(transmitters_list)
return (nbr_id, bearing, orientation, range_bits)
def smallest_angle_diff(current_angle, goal_angle):
# student code start
if (current_angle >= 0 and goal_angle >= 0) or (current_angle < 0
and goal_angle < 0):
return math2.normalize_angle(goal_angle - current_angle)
elif (current_angle >= 0):
return -math2.normalize_angle(abs(current_angle) + abs(goal_angle))
else:
return math2.normalize_angle(abs(current_angle) + abs(goal_angle))
def update():
global motion_state
if motion_state['motion_done'] == True:
return (0, 0)
(x_goal, y_goal) = motion_state['goal_pos']
(x_robot, y_robot, heading_robot) = pose.get_pose()
(distance_goal, heading_goal) = math2.topolar(x_goal - x_robot, y_goal - y_robot)
heading_goal = math2.normalize_angle(heading_goal)
if distance_goal < MOTION_CAPTURE_DISTANCE:
# you are at your destination
motion_state['motion_done'] = True
tv = 0
rv = 0
elif distance_goal > MOTION_RELEASE_DISTANCE:
motion_state['motion_done'] = False
if motion_state['motion_done'] == False:
# Drive towards goal position
tv = distance_goal * MOTION_TV_GAIN + MOTION_TV_MIN
tv = velocity.clamp(tv, motion_state['tv_max'])
# Rotate towards goal position
heading_error = math2.smallest_angle_diff(heading_robot, heading_goal)
rv = MOTION_RV_GAIN * heading_error
rv = velocity.clamp(rv, MOTION_RV_MAX)
if motion_state['rotate_only']:
tv = 0
if abs(heading_error) < MOTION_RELEASE_ANGLE:
motion_state['rotate_only'] = False
else:
if abs(heading_error) > MOTION_CAPTURE_ANGLE:
motion_state['rotate_only'] = True
return (tv, rv)
def pose_update(pose_state):
# 1. Get the left and right encoder ticks
ticks_left = rone.encoder_get_ticks('l')
ticks_right = rone.encoder_get_ticks('r')
# 2. Compute the left and right delta ticks
# Don't forget to use encoder_delta_ticks() to properly compute the change in tick values
delta_ticks_left = velocity.encoder_delta_ticks(ticks_left,
pose_state['ticksL'])
delta_ticks_right = velocity.encoder_delta_ticks(ticks_right,
pose_state['ticksR'])
# 3. compute the left and right distance for each wheel
# cast the delta ticks from step 2 to floats before you do the distance computation
dL = ENCODER_MM_PER_TICKS * (1.0 * delta_ticks_left)
dR = ENCODER_MM_PER_TICKS * (1.0 * delta_ticks_right)
# 4. save the left and right ticks to pose_state so we can measure the difference next time
pose_state['ticksL'] = ticks_left
pose_state['ticksR'] = ticks_right
# 5. Compute the distance traveled by the center of the robot in millimeters
dC = 0.5 * (dL + dR)
# 6. Add the distance to the odometer variable in pose_state
pose_state['odometer'] += abs(dC)
# 7. compute the arc angle in radians
# don't call atan here, use the small angle approximation: arctan(theta) ~ theta
theta = (dR - dL) / WHEEL_BASE
# 8. finally, update x, y, and theta, and save them to the pose state
# use math2.normalize_angle() to normalize theta before storing it in the pose_state
pose_state['theta'] = math2.normalize_angle(pose_state['theta'] + theta)
pose_state['x'] += dC * math.cos(pose_state['theta'])
pose_state['y'] += dC * math.sin(pose_state['theta'])
def avoid_nbr(nbr, tv):
beh_out = BEH_INACTIVE
if nbr != None:
bearing = math2.normalize_angle(neighbors.get_nbr_bearing(nbr) + math.pi)
rv = bearing * tv * TVRV_RATIO * RV_FOLLOW_GAIN
#tv = MOTION_TV
beh_out = (tv, rv, True)
return beh_out
def move_in_dir(bearing):
bearing = math2.normalize_angle(bearing)
tv = math.cos(bearing) * MOTION_TV
rv = math.sin(bearing) * MOTION_RV
if abs(bearing) > math.pi / 2:
rv = -rv
tv, rv = int(tv), int(rv)
return tv, rv
def avoid_nbr(nbr, tv):
beh_out = BEH_INACTIVE
if nbr != None:
bearing = math2.normalize_angle(
neighbors.get_nbr_bearing(nbr) + math.pi)
rv = bearing * tv * TVRV_RATIO * RV_FOLLOW_GAIN
#tv = MOTION_TV
beh_out = (tv, rv, True)
return beh_out
def get_avg_bearing_to_nbrs(nbr_list):
x = 0.0
y = 0.0
for nbr in nbr_list:
bearing = neighbors.get_nbr_bearing(nbr)
x += math.cos(bearing)
y += math.sin(bearing)
avg_bearing = math.atan2(y, x)
avg_bearing = math2.normalize_angle(avg_bearing)
return avg_bearing
def wall_follow(tv):
obs_angle = hba.obstacle_angle_get()
active = False
if (obs_angle != None):
alpha = math2.normalize_angle(obs_angle + math.pi/2)
rv = 900 * alpha
active = True
else:
# no wall. arc to the right to look for one
rv = -1000
return (tv, rv, active)
def bump_angle_get():
bumps_raw = rone.bump_sensors_get_value()
if bumps_raw == 0x00:
return None
else:
sum_x = 0
sum_y = 0
for i in range(8):
pressed = ((bumps_raw >> i) & 0x01) > 0
if pressed:
sum_y+= math.sin(SENSOR_ANGLES[i])
sum_x+= math.cos(SENSOR_ANGLES[i])
bump_angle = math2.normalize_angle(math.atan2(sum_y, sum_x))
return bump_angle
def bump_angle_get():
bumps_raw = rone.bump_sensors_get_value()
if bumps_raw == 0x00:
return None
else:
sum_x = 0
sum_y = 0
for i in range(8):
pressed = ((bumps_raw >> i) & 0x01) > 0
if pressed:
sum_y += math.sin(SENSOR_ANGLES[i])
sum_x += math.cos(SENSOR_ANGLES[i])
bump_angle = math2.normalize_angle(math.atan2(sum_y, sum_x))
return bump_angle
def flock_beh():
# act on the information from the message. Note that this might be
# information stored from the last message we received, because message
# information remains active for a while
nbr_list = neighbors.get_neighbors()
if len(nbr_list) > 0:
# any neighbor wil do. get the first neighbor
x = 0.0
y = 0.0
for nbr in nbr_list:
nbr_bearing = neighbors.get_nbr_bearing(nbr)
nbr_orientation = neighbors.get_nbr_orientation(nbr)
nbr_heading = math2.normalize_angle(math.pi + nbr_bearing - nbr_orientation)
x += math.cos(nbr_heading)
y += math.sin(nbr_heading)
nbr_heading_avg = math.atan2(y, x)
beh = (0, FLOCK_RV_GAIN * nbr_heading_avg, True)
else:
#no neighbors. do nothing
beh = (0, 0, False)
return beh
def pose_update(pose_state):
# 1. Get the left and right encoder ticks
left = rone.encoder_get_ticks("l")
right = rone.encoder_get_ticks("r")
# 2. Compute the left and right delta ticks
# Don't forget to use encoder_delta_ticks() to properly compute the change in tick values
dleft = velocity.encoder_delta_ticks(left, pose_state['ticksL'])
dright = velocity.encoder_delta_ticks(right, pose_state['ticksR'])
# 3. compute the left and right distance for each wheel
# cast the delta ticks from step 2 to floats before you do the distance computation
dl = float(dleft) * ENCODER_MM_PER_TICKS
dr = float(dright) * ENCODER_MM_PER_TICKS
# 4. save the left and right ticks to pose_state so we can measure the difference next time
pose_state['ticksL'] = left
pose_state['ticksR'] = right
# 5. Compute the distance traveled by the center of the robot in millimeters
center = (dr + dl) / 2.0
# 6. Add the distance to the odometer variable in pose_state
pose_state['odometer'] = pose_state['odometer'] + abs(center)
# 7. compute the arc angle in radians
# don't call atan here, use the small angle approximation: arctan(theta) ~ theta
dtheta = (dr - dl) / float(WHEEL_BASE)
# 8. finally, update x, y, and theta, and save them to the pose state
# use math2.normalize_angle() to normalize theta before storing it in the pose_state
l = ((dr - dl) / 2.0) * math.sin(90 - dtheta)
ntheta = pose_state['theta'] + dtheta
pose_state['x'] = (center + l) * math.cos(ntheta) + pose_state['x']
pose_state['y'] = (center + l) * math.sin(ntheta) + pose_state['y']
pose_state['theta'] = math2.normalize_angle(ntheta)
return 0
def update():
global motion_state
if motion_state['motion_done'] == True:
return (0, 0)
(x_goal, y_goal) = motion_state['goal_pos']
(x_robot, y_robot, heading_robot) = pose.get_pose()
(distance_goal, heading_goal) = math2.topolar(x_goal - x_robot,
y_goal - y_robot)
heading_goal = math2.normalize_angle(heading_goal)
if distance_goal < MOTION_CAPTURE_DISTANCE:
# you are at your destination
motion_state['motion_done'] = True
tv = 0
rv = 0
elif distance_goal > MOTION_RELEASE_DISTANCE:
motion_state['motion_done'] = False
if motion_state['motion_done'] == False:
# Drive towards goal position
tv = distance_goal * MOTION_TV_GAIN + MOTION_TV_MIN
tv = velocity.clamp(tv, motion_state['tv_max'])
# Rotate towards goal position
heading_error = math2.smallest_angle_diff(heading_robot, heading_goal)
rv = MOTION_RV_GAIN * heading_error
rv = velocity.clamp(rv, MOTION_RV_MAX)
if motion_state['rotate_only']:
tv = 0
if abs(heading_error) < MOTION_RELEASE_ANGLE:
motion_state['rotate_only'] = False
else:
if abs(heading_error) > MOTION_CAPTURE_ANGLE:
motion_state['rotate_only'] = True
return (tv, rv)
def update():
global pose_state
current_time = sys.time()
update_time = pose_state['update_time']
if current_time > update_time:
update_time += POSE_UPDATE_PERIOD
# advance time if there have been delays in calling update
if update_time < current_time:
update_time = current_time + POSE_UPDATE_PERIOD
pose_state['update_time'] = update_time
# Compute the linear distance traveled by each wheel in millimeters
ticksL = rone.encoder_get_ticks('l')
ticksR = rone.encoder_get_ticks('r')
distL = float(velocity.encoder_delta_ticks(ticksL, pose_state['ticksL'])) * 0.0625
distR = float(velocity.encoder_delta_ticks(ticksR, pose_state['ticksR'])) * 0.0625
pose_state['ticksL'] = ticksL
pose_state['ticksR'] = ticksR
# Compute the distance traveled by the center of the robot in millimeters
dist = (distR + distL) / 2.0
pose_state['odometer'] += abs(dist)
# compute the arc angle in radians
# use the small angle approximation: arctan(theta) ~ theta
delta_theta = (distR - distL) / (WHEEL_BASE);
# (x, y, theta) = pose_state['pose']
# x = x + dist * math.cos(theta)
# y = y + dist * math.sin(theta)
# theta = math2.normalize_theta(theta + delta_theta)
# pose_state['pose'] = (x, y, theta)
theta = pose_state['theta']
pose_state['x'] += dist * math.cos(theta)
pose_state['y'] += dist * math.sin(theta)
pose_state['theta'] = math2.normalize_angle(theta + delta_theta)
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 match_nbr_heading(nbr):
nbr_brg = neighbors.get_nbr_bearing(nbr)
nbr_ornt = neighbors.get_nbr_orientation(nbr)
heading_error = math2.normalize_angle(math.pi + nbr_brg - nbr_ornt)
rv = ROTATE_RV_GAIN * heading_error
return (rv, heading_error)
def match_nbr_heading(nbr):
nbr_brg = neighbors.get_nbr_bearing(nbr)
nbr_ornt = neighbors.get_nbr_orientation(nbr)
heading_error = math2.normalize_angle(math.pi + nbr_brg - nbr_ornt)
rv = ROTATE_RV_GAIN * heading_error
return (rv, heading_error)
