На форуме много примеров собственных П, ПИ и ПИД регуляторов. Также есть библиотеки КДС и Овен с примерами.
Лучше попробуйте готовые решения.

Из написанного Вами мало, что понятно.