Тогда можете ознакомиться с примерами реализации ПИД-регулятора по ссылке: http://www.owen.ru/forum/showthread.php?t=13588