Velocity-based PID with the WPILib PIDController Class

[Mike Copioli]

What kind of speed controllers are you using? This non linear response suggests that you are using Victors. Is this the case? I would still advise you to withhold the I and D gains until you have established that your code is correct. P is very simple to debug. PI is not so easy. PID is even harder yet. If you are using Windriver place your input (throttle), Verror, and PID output variables into the expressions view. Insert a breakpoint on the next executable line of code after the PID machine. Compare the resultant values, since P is effectively P output = (Pk*Verror)*throttle (capped at 1/-1) you should be able to do the math yourself and determine if the values make sense. The math example above is how we do P you should verify that the wpi libs do it the same way. Some add or subtract. Either way the point is that as Verror approaches 0 your P output should decrease proportionally to the error, hence the name proportional gain.