Problema funzione evita ostacoli e miglioramento del codice

Salve a tutti, da un po di tempo porto avanti un piccolo progetto di un robot rover con vari sensori e varie funzioni, fino ad ora ho scritto tutto il codice da solo cercando ogni tanto alcune cose che non conoscevo sul sito di Arduino o su internet, ho avuto molti problemi con la programmazione e ancora ad oggi ne ho, quindi ho deciso di chiedere aiuto a persone più esperte di me visto che io comunque sono ancora alle basi della programmazione e al momento mi sono bloccato, adesso veniamo al problema, sulla piccola macchina ho alcuni sensori ad ultrasuoni HC-SR04 per rilevare gli ostacoli, in totale al momento ne ho 3, una posto alla destra, uno posto alla sinistra e uno posto al centro montato su un micro servo, ho poi scritto una funzione chiamata: AUTONOMOUS MODE che ha tre funzioni, la prima (AUTONOMOUS MODE 1) mi permette di rilevare gli ostacoli mediante il sensore centrale posto sul micro servo, quando rileva uno ostacolo il servo ruota rispettivamente a 180/90/0 gradi salvando i valori delle distanze in delle variabili per poi fare la differenza e scegliere la strada migliore, fino a qui tutto bene, adesso però ho problemi nella funzione AUTONOMOUS MODE 2 che in pratica dovrebbe rilevare gli ostacoli da tutti e 3 i sensori e muoversi di conseguenza evitando gli ostacoli, però vorrei che quando si avvicini sempre di più ad un ostacolo rallenti per poi riprendere la normale velocità anche per non stressare troppo i motori ed il ponte H, metto tutto il codice completo per avere consigli su come potrei migliorarlo il codice e per risolvere il problema che vi ho descritto.

Saluti
Davide.

ARDUROVER_MEGA_1_2_6.zip (10.1 KB)

Salve, nessuno sa aiutarmi? sono praticamente bloccato con il codice non riesco ad andare avanti.

Lo sketch mi pare lunghetto, mettersi ad esaminarlo tutto è problematico.... oltretutto pilota qualcosa di specifico... Se non riesci ad isolare il problema, difficilmente in molti si metteranno ad esaminare riga per riga il programma...