Arduino code to calculate motor position using a quadrature encoder