PID controller integral term causing extreme instability

Question

I have a PID controller running on a robot that is designed to make the robot steer onto a compass heading. The PID correction is recalculated/applied at a rate of 20Hz.

Answer 1

Are you initializing SteadyError (bizarre name...why not "integrator")? If it contains some random value on start-up it might never return to near zero (1e100 + 1 == 1e100).

You might be suffering from integrator windup, which ordinarily should go away, but not if it takes longer to diminish than it does for your vehicle to complete a full rotation (and windup the integrator again). The trivial solution is to impose limits on the integrator, though there are more advanced solutions (PDF, 879 kB) if your system requires.

Does Ki have the correct sign?

I would strongly discourage the use of floats for PID parameters because of their arbitrary precision. Use integers (maybe fixed point). You will have to impose limit checking, but it will be much more sane than using floats.