Il tuo sketch è pieno zeppo di delay che bloccano il proseguirsi del programma. La fusione di due sketch può essere un impresa ardua, in molti casi conviene quasi riscrivere tutto da zero.
Esistono però degli approcci ponderati per sviluppare un software di controllo di un MCU che debba svolgere diverse funzioni.
Non esiste un sistema unico che funziona sempre, ma suddividere il programma in step fondamentali può essere una buona base di partenza.
Un esempio, ho 3 sensori generici, 2 servo motori inoltre ho una connessione seriale su cui inviare i dati raw dei sensori.
Posso pensare di scrivere 3 routine per la raccolta dati, ognuna si occuperà di leggere dati da un singolo sensore e salvarli nella memoria di Arduino, una routine di aggiornamento dei servi e una per l'invio dei dati via seriale. Se questa fase preliminare non ha presentato particolari problemi posso dedicarmi alla fase di scheduling, ovvero compiere manualmente il lavoro che fa uno sheduler su un normale PC, non so se hai presente, il computer può fare solo un operazione alla volta, ma se è abbastanza veloce ed intelligente può farti credere di fare più cose contemporaneamente(furbacchione 
). Quindi inizio a far lavorare la testa e penso alle precedenze, inviare un dato prima di averlo acquisito non si puo fare, quindi sicuramente prima di richiamare la routine di invio dati dovrò eseguire le 3 routine dei sensori. Discorso simile per i servo, quindi inizio a delineare un grafo delle precedenze e un ordine preliminare.
-Leggo sensore 1
-Leggo sensore 2
-Leggo sensore 3
-Aggiorno i servi
-Invio i dati
Poi ci penso su e riflettendo sul sensore 1 che è un NTC, rispetto agli altri due sensori che sono sensori di prossimità capisco che un aggiornamento dei dati alla medesima velocità ha poco senso, quindi posso dire, i sensori 2 e 3 si aggiornano ogni ciclo di loop, il sensore 1 solo ogni 4 cicli. Posso fare anche un confronto tra i servi e l'invio dei dati. Magari l'invio dei dati è solo di debug, invece l'aggiornamento dei servi è un controllo attivo, quindi potrei dire, i servi li aggiorno ad ogni ciclo, i dati li invio ogni 10(tanto io sono lento a leggerli).
Cosi ho delineato una schedulazione soddisfacente e sensata. Vorrei farti notare una cosa, non ho mai parlato di aspettare, generalmente tutte le operazioni che eseguo dovrebbero occupare il micro il minor tempo possibile, cosi da farci credere che sappia fare piu cose contemporaneamente. I delay sono il male, impara a lavorare con la funzione mills(), che permette di sviluppare sistemi di timing non bloccanti.
Spero di esserti stato utile.