I am implementing feedforward for the rotation component of a pink arm but I am encountering steady-state error with a PID controller. Since the length of the arm (which impacts the effect of gravity on the rotation) varies, I am not using the ArmFeedforward class from WPILib, but I am calculating the feedforward using the calculation on line 58. All of the units in my feedforward implementation are in volts. I am testing using a simulated arm because I do not have around-the-clock access to an actual robot.
Previously, with only a PID controller, the simulated arm was able to consistently rotate until the simulated arm angle was equal to the setpoint arm angle. The addition of gravity compensation using the feedforward voltage calculator caused this feature to break, with the simulated arm angle rotating to a few degrees above the setpoint angle and being unable to progress further. Here is the whole Shuffleboard tab containing the data from the simulated arm when it is stuck, and the lists containing the arm position and setpoint position:
Here’s a visualization of the arm position when it is stuck, using a mechanism:
The implementation of the arm feedforward that causes this behavior is this: the feedfoward calculates the voltage needed to resist the force of gravity at the current position (e.g. keep the arm at that position), then the PID controller calculates a voltage required to move the arm towards the setpoint, then these voltages are added together (which accomplishes staying at the setpoint if not at the setpoint, otherwise moving towards the setpoint) and sent to the simulated motor.
The calculation to resist the force of gravity is at: https://github.com/haydenheroux/Thoth/blob/0a30dc16b02559493d1fd121ca1c1182f5bb8a7e/src/main/java/frc/robot/arm/ExtensionRotationFeedforward.java#L32
The constant in there was found by setting the arm’s input rotation voltage to zero, then increasing the kG variable until the arm stayed still in one position. This is the same methodology from the WPILib article on tuning an arm position controller.
The PID calculation (for the simulated arm) is at: https://github.com/haydenheroux/Thoth/blob/0a30dc16b02559493d1fd121ca1c1182f5bb8a7e/src/main/java/frc/robot/arm/ArmIOSim.java#L121
The summation of the voltages and using the voltage with the simulated arm is at: https://github.com/haydenheroux/Thoth/blob/0a30dc16b02559493d1fd121ca1c1182f5bb8a7e/src/main/java/frc/robot/arm/ArmIOSim.java#L134 and https://github.com/haydenheroux/Thoth/blob/0a30dc16b02559493d1fd121ca1c1182f5bb8a7e/src/main/java/frc/robot/arm/ArmIOSim.java#L67
What I am stuck on is the fact that the feedforward calculation produces the correct amount of voltage to hold the simulated arm at an angle when input voltage is zero (and the simulated brake is disabled, which was separately tested to be okay), and that the PID controller produces a value that is able to reduce the error by a considerable amount before becoming stuck at the steady-state position a few degrees above the setpoint. Here the values I am getting for each calculation when the arm is stuck at the steady-state position:
- inputVoltage is the voltage from the PID controller, attempting to match the setpoint
- voltageToOvercomeGravity is what the feedforward controller calculates is the voltage needed to resist gravity (hold the arm at the angle) for the arm’s position
- outputVoltage is the voltage sent to the motor, controlling the arm angle
Since the output voltage is less than the voltage needed to overcome gravity, shouldn’t the arm angle decrease, instead of staying still?
Also, it doesn’t seem like a coincidence that the stuck state occurs when the input and output voltages are opposites. What does that have to do with the angle of the arm not changing, when only the output voltage is being sent to the motor?