tachus42,
I'm a little bit confused on your 40Khz encoder update. I have some 500 line encoders, with 4x quadrature, that is 2000counts/per rev. I calculate a maximum rpm of about 1200. Have you done some testing to see what the real world max rpm is without any miss counts? There seems to be a much greater chance of missing counts when polled vs. interrupts.
I quickly looked over your code but since it isn't commented, I couldn't easily tell if you are doing 1x or 4x quadrature decoding.
Jim