Servo jitters when using interrupts to read quadrature encoder