Qui però bisogna scegliere: se lavori con i registri il codice è veloce ma non portabilissimo.
Se lavori con le funzioni definite da Arduino ad un livello più alto, il codice è più portabile ma un pò più lento.
Poi sinceramente non ho capito perché incapsuli le funzioni wait_20ms o wait_5ms.
Non fai prima a scrivere nel codice il delay e la chiamata alla funzione di lettura senza rimandare ad una funzione che ne chiama un'altra.
Sono d'accordo anche io per la modularizzazzione, ma quando è troppo....
Poi, comunque, il compilatore ottimizza, ma il codice diventa illeggibile.