def encoder_pid_processing(cur_time, cur_ticks, pid_data):
update = {}
if pid_data['prev_time'] is None:
pass # Just copy the ticks and time if there's no data at all
elif pid_data['prev_velocity'] is None:
# Just calculate the velocity
dt = (cur_time - pid_data['prev_time']).to_sec()
update['prev_velocity'] = controller.encoder_to_velocity(
cur_ticks,
pid_data['prev_ticks'],
dt,
)
else:
# Figure out the current velocity
dt = (cur_time - pid_data['prev_time']).to_sec()
cur = controller.encoder_to_velocity(cur_ticks, pid_data['prev_ticks'], dt)
# Put everything into the PID controller
pid_output = controller.pid(REF_SPEED, cur, pid_data['prev_velocity'],
pid_data['error_integral'],
pid_data['prev_error'], dt, K_P, K_I, K_D)
# Record everything for the next iteration
update['prev_error'] = pid_output['e']
update['error_integral'] = pid_output['int']
update['linear_velocity_cmd'] = pid_output['out']
update['prev_velocity'] = cur
update['prev_ticks'] = cur_ticks
update['prev_time'] = cur_time
return update
def encoder_callback(msg):
"""Callback to handle encoder odometry information.
Uses a PID controller to control the velocity of the robot.
"""
# Setup globals
# TODO(mkwan): Make it not rely on globals
global REF_SPEED
global K_P
global K_I
global K_D
global error_integral
global prev_time
global prev_velocity
global prev_ticks
global prev_error
global linear_velocity_cmd
# Get the current time, and the number of ticks recorded
cur_time = rospy.get_time()
cur_ticks = msg.ticks[0]
# Logging the info
rospy.loginfo("ENCODER:I got: [%d] as encoder ticks at [%f]", cur_ticks, cur_time)
if prev_time is None:
pass # Just copy the ticks and time if there's no data at all
elif prev_velocity is None:
# Just calculate the velocity
dt = cur_time - prev_time
prev_velocity = controller.encoder_to_velocity(cur_ticks, prev_ticks, dt)
else:
# Figure out the current velocity
dt = cur_time - prev_time
cur = controller.encoder_to_velocity(cur_ticks, prev_ticks, dt)
# Put everything into the PID controller
pid_output = controller.pid(REF_SPEED, cur, prev_velocity, error_integral, prev_error, dt, K_P, K_I, K_D)
# Record everything for the next iteration
prev_error = pid_output["e"]
error_integral = pid_output["int"]
linear_velocity_cmd = pid_output["out"]
prev_velocity = cur
prev_ticks = cur_ticks
prev_time = cur_time
# Write the encoder tick data to file
if not prev_velocity is None:
writer = csv.writer(open("/home/administrator/ROS_packages/grp6_lab1/nodes/data.csv", "a"))
writer.writerow((cur_time, cur_ticks, linear_velocity_cmd, prev_error, error_integral, prev_velocity))
