I believe what you observe with the integral part is windup. You might fix that by limiting the overall value of the I part, or resetting it once the error is small or zeroes out or changes sign (zero crossing). All are known approaches dealing with windup (basically the integral keeps collecting until it saturates and then the system becomes unstable). Gil

Only top voted, non community-wiki answers of a minimum length are eligible