Используйте PID регулятор из библиотеки Util.lib. С ним проще разобраться, работает как и должен работать PID регулятор. Понятно и предсказуемо.