Возьмите пид-регулятор и не занимайтесь ерундой