Velocity-based PID with the WPILib PIDController Class

[Mike Mahar]

I seems as though you are trying to do a positional PID when you want a velocity PID. For a positional PID, the motor will start out fast and then slow down to 0 when you get to your position. If you do this for speed, however, as the indicated speed goes up, your PID will start to slow down and your speed will drop which will cause the PID to speed up the motors again. With some values, it may stabilize at about 1/2 power.

For a velocity PID, you don't control the motors directly with the PID. Instead you control the amount you want to change the speed of the motor. This variable is adjusted like a motor control but it is added to the current motor speed. Suppose the current motor power is 0. If the desired speed in 100, for example, the PID may set the delta at 0.1 (depending on the kP value) 0.1 will added to the motor power each time through the PID loop. As the speed of the robot approaches 100 the delta will be reduced untill it reaches 0.0. at this point the power of he robot will be at whatever value is needed to sustain the desired speed.