Микроконтроллеры

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