ProfiledPIDController starts with a value, and then slowly lowers down to zero

I’m trying to use a ProfiledPIDController to control our pivoting arm. The thing is that even though I set a very low P value and when I just start the robot, the result from the calculate() is not zero. The arm is not moving, and it has the same position as the encoder. The thing is that at the start the value is high when the arm is at 0 and setpoint at 0, and over time this value lowers back to zero.

What am I doing wrong?

It’s hard to say from this description. Do you observe the same behaviour with PIDController? Can you share a link to your code?

Could you link your code?

Yeah, sorry. Here is the link to our code.


No, the PIDController works fine

I recommend that you put setpoint, actualPoint, error, pidOut, feedforwardValue and total on the SmartDashboard, graph them, and post a screenshot.

Sure, here they are. Arm Output is pidOut. Desired position is setpoint. Arm Position is actualPoint. Error is error. Feed Output is feedforwardValue.

I think your PID controller is working in degrees, right? This means that your trapezoid profile is set to limit it to 2 degrees per second. Your range of motion is 291°, so that will take over two minutes to move the arm between extremes.

Yes, I’ve increased the speed and acceleration of the motion profile, and it has reduced the time for the pid to reach 0 at the start of the robot code. Are there any way to avoid this entirely?

You talk about zero, but both your initial setpoint and initial position are -148.7, right? What position is the arm in initially?

Assuming that 0° is straight up, your feedforward should be based on -sin(theta).

I don’t really understand why you would be getting non-zero feedback output when the setpoint and current position are the same, but it might help to call profiledController.reset(getMeasurement(), 0);. (I’d suggest getVelocity() instead of zero, but that doesn’t seem to be in degrees per second.)

Also had this issue. It always and only happens on startup.

Yeah, initial setpoint and position is -148.7. The arm is “tucked” into the robot. 0 degrees is straight up. Okay, I’ll do -sin(theta) for the feedforward, that might be wayy better than the current solution. I’ll try calling the reset method, though I’m not sure it will work as that just resets the integral term, but I’m not using any, but I’ll try it out either way.

Yeah, I have to wait a couple seconds for the PID to go to zero. Have you found any solutions?

I think you need a reset controller call prior to your initial calculateArm. If I understand correctly this resets the error and allows the profiled PID controller to know the current position so it can do the math on the profiled motion.

it would look something like


It just needs to be called once before you start doing your initial profiled motion. This seemed to solve our issues that were similar. We currently have made our profiled motions into a command and just have the initialize method be the reset controller and the execute run the profiledcontroler and set the motors.

Seems to work better otherwise we would get some wild things happen when we first ran the profiledController.

 * Implements a PID control loop whose setpoint is constrained by a trapezoid profile. Users should
 * call reset() when they first start running the controller to avoid unwanted behavior.

It seems as even WPILIB knows this, so yeah, as a solution y should run profiledController.reset(position, 0); at the start

1 Like