WPILib ProfiledPID command with TalonFX and Rev Through bore encoder incorrect error and output

Our team has been trying to use a PID and Trapezoidal motion profiled method for controlling our arm. We have a falcon with gearing on it and a Rev Through bore encoder on the final output shaft configured in absolute going through DIO ports and using the DutyCycleEncoder. Initially we used TalonFX Motion Magic but we had issues with setSelectedSensorPosition function not updating the talon fast enough and it was lagging and causing voltage issues.
Currently we are trying to use the ProfiledPID command from wpilib but are having issues with it.
Link to command we have: https://github.com/WHS-FRC-3467/Skip-5.13/blob/master/src/main/java/frc/robot/subsystems/arm/ArmRioPID.java
The magnitude of the error and calculate(…) function equals the encoder measurement. We have tried a few different ways of setting the goal addressing setpoints, starting the arm in different positions relative to the setpoint and inverting signs but have had no luck. For example in case 1 of the table below if the setpoint is above where the arm is currently it will drive away from it but if the arm is above where the setpoint is it will drive to the setpoint then past it without the error getting anywhere close to 0 near the setpoint.
Here are the results we’ve had

Here are a few of the log files

edit: ignore the PID calculated output part, P was set to 1.0 so that makes sense. The issue has more to do with the error
edit 2: We tried with a standard PID command and that worked as expected.

We have pinpointed the issue to the ProfiledPID command outputting a velocity to the PID command not a position, so the PID controller is calculating based off a positional setpoint for a setpoint and a velocity output as the measurement.

   * Returns the next output of the PID controller.
   * @param measurement The current measurement of the process variable.
   * @return The controller's next output.
  public double calculate(double measurement) {
    if (m_controller.isContinuousInputEnabled()) {
      // Get error which is the smallest distance between goal and measurement
      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
      double goalMinDistance =
          MathUtil.inputModulus(m_goal.position - measurement, -errorBound, errorBound);
      double setpointMinDistance =
          MathUtil.inputModulus(m_setpoint.position - measurement, -errorBound, errorBound);

      // Recompute the profile goal with the smallest error, thus giving the shortest path. The goal
      // may be outside the input range after this operation, but that's OK because the controller
      // will still go there and report an error of zero. In other words, the setpoint only needs to
      // be offset from the measurement by the input range modulus; they don't need to be equal.
      m_goal.position = goalMinDistance + measurement;
      m_setpoint.position = setpointMinDistance + measurement;

    var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
    m_setpoint = profile.calculate(getPeriod());
    return m_controller.calculate(measurement, m_setpoint.position);

   * Returns the next output of the PIDController.
   * @param measurement The current measurement of the process variable.
   * @param goal The new goal of the controller.
   * @return The controller's next output.
  public double calculate(double measurement, double goal) {
    return calculate(measurement);

It seems like an error or oversite in the WPILib but we are unsure how to correct for this.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.