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