Я бы добавил внешний контур управления на четкой программной
логике. Типа если что-то уперлось в порог, то, возможно,
корректируем алгоритм регулирования. Также отдельно прописываем
процедуры старта и завершения, чтобы только в области относительно
малых отклонений автоматически регулировать.