Premetto che non ho mai utilizzato Arduino.
Posso suggerirti però di iniziare a ragionare a "stati", ovvero creare una funzione che esegue poche operazioni, quindi cambia un suo stato interno per il passo successivo: richiamando continuamente questa e anche altre funzioni simili dal ciclo principale permetterai al tuo sistema di gestire più compiti contemporaneamente.
In pratica devi trasformare il tuo thread dove fai le operazioni una dietro l'altra in una funzione che consiste di un "switch" dove per ogni "case" fai una parte del thread ed esci subito.
Se poi vuoi eseguire delle operazioni con tempistiche più precise inserisci queste operazioni all'interno di un timer.
Spero di esserti stato d'aiuto.