El código que se utilizo se baso en usar dos interrupciones principales para hacer el calculo de la posición y la velocidad para esto se usó una interrupción por tiempo cada 1 ms y una por GPIO para cada vez que llegue un pico, y se creó un función de atención a cada una de estas interrupciones.
El codigo usado se puede encontrar en Github