Какой не какой ПИД на STM32F103

[Uvavan]

Здравствуйте!
Делаю для собственного удовольствия и собственных нужд ПИД регулятор (возможно и жалкое подобие ). Регулятор для двигателя на постоянке, управление через ШИМ таймера 2 канал 4. Пользовался ссылками 1 и 2 и 3
Код:

 E=RevN - RevT;
        E=E*(-1);
        //E=E/334;
        if (E!=0) 
        {    
            U=0;
            U=Kp*E;
            I+=E;
            if (I ›=4000)
            {
                I=4000;
            }
            if (I‹=(-4000))
            {
                I=-4000;
            }
            U+=Ki*I;
            U+=Kd*(E-Eprev);
            t=TIM2-›CCR4;
            U+=t;
            if (U›=5000)
            {
                U=5000;
            }    
            TIM2-›CCR4=U;
        }
        Eprev=E; 

E=E*(-1); для инвертирования значения ошибки так как при уменьшение скорости период от оптопары увеличиваться а не уменьшается.
Если я правильно понял работу ПИДа при прикладывание усилия на вал двигателя (и его замедление) величина ШИМ увеличивается до достижения заданных оборотов.
Как работает у меня. Заполнение увеличиваться но достигает какого то значения, останавливается и немного "прыгает вокруг" этого значения, хотя заданная скорость не достигнута. До ограничения 5000 единиц не доходит.
Это правильная работа регулятора?

[Boba_spb]

E=E*(-1); для инвертирования значения ошибки

Вместо этого, я б посоветовал в первой строке E=RevT - RevN;