Введи идентификатор в цепь обратной связи.
Поставь ПИД с заведомо "мягкими" параметрами, а по ходу работы вычисляй можно ли "подзажать" или нужно отпустить.