You have quite high integral gain I'm surprised the system doesn't become unstable with that.
I'd try setting Ki and Kd to 0, then increase Kp until you're starting to get oscillation then back Kp off a bit. At this point I suspect that you're not quite reaching the setpoint so start increasing Ki. The intergral term will help "catch up" the part that the proportional alone can't do.


How often do you run the loop?