да не правиш управление на серво мотор за цнц
ако е така (а дори и за друго да е) 2000 не са много, най вероятно програмата не ти е оптимизирана, но вземи си процесор със вход за енкодер например 18ф4431 и проблемите ти отпадат - хардуерен брояч който го четеш от време на време (при мен 2кхз). има и много други с такъв вход търси quadratic encoder

другия вариант виж application notes на микрочип имат решения с 18ф442 ако не се лъжа и дори със по слаби, с някакъв д тригер май беше