Сделал обнуление если сервопривод выключен. Проблема осталась.Т.е. иногда сервопривод проскакивает условие.У меня конечно есть мысли почему это так,но не знаю прв ли я.Этот кусок кода который я привел в начале вопроса вызывается через функциональный блок.И я думаю что на вызов и выполнение функционального блока уходит куда больше времени, чем на считывание импульсов с энкодера. И из-за этого программа просто не успевает отреагировать.