А чем не устроил ПИД регулятор (представлен в виде функционального блока библиотеки Кодесис)?