Não vou ter tempo nesta semana, pra analisar seu código.
Mas recentemente demonstrei algo semelhante aqui no LDG.
Então, assim como vc "deixou o código", vou também deixar o link para esta implementação, pois a partir dali (se vc dominar relativamente "bem" a programação no Arduino), vc conseguirá fazer isso que descreveu (isso é possível sim, sabendo o que fazer corretamente no hardware e firmware).