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 init():
global pose_state
pose_state['ticksL'] = rone.encoder_get_ticks('l')
pose_state['ticksR'] = rone.encoder_get_ticks('r')
#pose_state['pose'] = (0.0, 0.0, 0.0)
pose_state['x'] = 0.0
pose_state['y'] = 0.0
pose_state['theta'] = 0.0
pose_state['odometer'] = 0.0
pose_state['update_time'] = sys.time()
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 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 init(kff, kff_offset, kp, ki):
_vcstate['l'] = {}
_vcstate['r'] = {}
# K terms
_vcstate['kff'] = kff
_vcstate['kff_offset'] = kff_offset
_vcstate['kp'] = kp
_vcstate['ki'] = ki
_vcstate['update_time'] = sys.time()
_vcstate['tv_ramp'] = 0
_vcstate['tv_update_time'] = sys.time()
for motor in ['l', 'r']:
_vcstate[motor]['ticks'] = rone.encoder_get_ticks(motor) # Position
_vcstate[motor]['time'] = sys.time() # Time
_vcstate[motor]['iterm'] = 0.0 # Iterm
_vcstate[motor]['goalvel'] = 0 # Goal velocity
_vcstate[motor]['vel'] = 0 # Current velocity
def init(kff, kff_offset, kp, ki):
_vcstate["l"] = {}
_vcstate["r"] = {}
# K terms
_vcstate["kff"] = kff
_vcstate["kff_offset"] = kff_offset
_vcstate["kp"] = kp
_vcstate["ki"] = ki
_vcstate["update_time"] = sys.time()
_vcstate["tv_ramp"] = 0
_vcstate["tv_update_time"] = sys.time()
for motor in ["l", "r"]:
_vcstate[motor]["ticks"] = rone.encoder_get_ticks(motor) # Position
_vcstate[motor]["time"] = sys.time() # Time
_vcstate[motor]["iterm"] = 0.0 # Iterm
_vcstate[motor]["goalvel"] = 0 # Goal velocity
_vcstate[motor]["vel"] = 0 # Current velocity
def init(kff, kff_offset, kp, ki):
_vcstate['l'] = {}
_vcstate['r'] = {}
# K terms
_vcstate['kff'] = kff
_vcstate['kff_offset'] = kff_offset
_vcstate['kp'] = kp
_vcstate['ki'] = ki
_vcstate['update_time'] = sys.time()
_vcstate['tv_ramp'] = 0
_vcstate['tv_update_time'] = sys.time()
for motor in ['l', 'r']:
_vcstate[motor]['ticks'] = rone.encoder_get_ticks(motor) # Position
_vcstate[motor]['time'] = sys.time() # Time
_vcstate[motor]['iterm'] = 0.0 # Iterm
_vcstate[motor]['goalvel'] = 0 # Goal velocity
_vcstate[motor]['vel'] = 0 # Current velocity
def _velocity(motor):
## vel_goal in mm/s
## motor either 'l' or 'r'
## iterm_old passed in from previous function runs (sum of the errors * ki)
## ticks_old is the old position of the given wheel
## time_old is the time of the last reading of the encoders
vel_goal = _vcstate[motor]['goalvel']
iterm_old = _vcstate[motor]['iterm']
ticks_old = _vcstate[motor]['ticks']
time_old = _vcstate[motor]['time']
# compute distance
ticks_new = rone.encoder_get_ticks(motor)
distance = _compute_distance(ticks_new, ticks_old)
# compute velocity_controller
time_new = sys.time()
time_delta = time_new - time_old
velocity = _compute_velocity(distance, time_delta)
_vcstate[motor]['vel'] = velocity
# some debug printing. Don't leave it in, it slows down the computer
# compute feedback terms
error = vel_goal - velocity
feedforward_term = _feedforward_compute(vel_goal)
proportional_term = _proportional_compute(error)
integral_term = _integral_compute(error, iterm_old)
pwm = feedforward_term + proportional_term + integral_term
pwm = int(pwm)
pwm = clamp(pwm, 100)
rone.motor_set_pwm(motor, pwm)
#Some example debugging output. Don't print this always, it will slow things down too much
#print 'motor=%s ff_term=%5.1f i_term=%5.1f pwm=%3d' % (motor, float(feedforward_term), float(integral_term), pwm)
# update old values
_vcstate[motor]['ticks'] = ticks_new
_vcstate[motor]['time'] = time_old = time_new
_vcstate[motor]['iterm'] = integral_term
def _velocity(motor):
## vel_goal in mm/s
## motor either 'l' or 'r'
## iterm_old passed in from previous function runs (sum of the errors * ki)
## ticks_old is the old position of the given wheel
## time_old is the time of the last reading of the encoders
vel_goal = _vcstate[motor]["goalvel"]
iterm_old = _vcstate[motor]["iterm"]
ticks_old = _vcstate[motor]["ticks"]
time_old = _vcstate[motor]["time"]
# compute distance
ticks_new = rone.encoder_get_ticks(motor)
distance = _compute_distance(ticks_new, ticks_old)
# compute velocity_controller
time_new = sys.time()
time_delta = time_new - time_old
velocity = _compute_velocity(distance, time_delta)
_vcstate[motor]["vel"] = velocity
# some debug printing. Don't leave it in, it slows down the computer
# compute feedback terms
error = vel_goal - velocity
feedforward_term = _feedforward_compute(vel_goal)
integral_term = _integral_compute(error, iterm_old)
proportional_term = _proportional_compute(error)
pwm = feedforward_term + proportional_term + integral_term
pwm = int(pwm)
pwm = clamp(pwm, 100)
rone.motor_set_pwm(motor, pwm)
# Some example debugging output. Don't print this always, it will slow things down too much
# print 'motor=%s ff_term=%5.1f i_term=%5.1f pwm=%3d' % (motor, float(feedforward_term), float(integral_term), pwm)
# update old values
_vcstate[motor]["ticks"] = ticks_new
_vcstate[motor]["time"] = time_old = time_new
_vcstate[motor]["iterm"] = integral_term
