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