Il codice ci va giù pesante, in fatto di velocità. Prova mettendeo dei delay e facendo debug sulla seriale per vedere se p buggato lui 
Come scrissi quando lo pubblicai, quel codice non l'ho potuto provare perché buttato giù senza avere un Arduino per cui potrebbe anche essere lui il problema 