Encoders- doble PID

Hola amigos,
Estoy haciendo un robot autobalanceado. Lo he hecho mediante un Arduino UNO, un MPU 6050 y un par de motores-reductores de esos amarillos (de los baratos). Luego por medio de un PID he conseguido que se estabilice y se mantenga derecho, eso sí, no para de moverse atrás y adelante. Como quiero evitar al máximo ese movimiento, he estado intentando e investigando varias opciones. Por esta razón, he visto en muchos sitios web que para un perfecto equilibrio se recomienda un PID en cascada formado por el MPU + un encoder. Total, primero compré un encoder-optoacoplador, luego el encoder rotativo KY-040 y ambos me daban problemas tanto de debouncing cómo de 'FIFO Overflow’. Es decir, para evitar el debouncing se necesita del attachinterrupt y de un debouncing por hardware. Eso genera ineficiencia en el código. En fin, me pregunto si ustedes me pueden ayudar/orientar, facilitaándome de alguna web o vídeo del empleo de un doble PID como el que quiero hacer. Además, desearía saber si hay mejores motores para medir las rpm preferiblemente con encoders, puesto que mi intención es hacer algún día, un Segway personal.
Gracias