Ton code est très long et difficile à lire : fais un CTRL T dans l'IDE et remplace ton listing par le nouveau, bien indenté STP.
A priori il reste encore des fautes, sauf si c'est bien ce que tu veux faire. Ceci par exemple :
while ( digitalRead(ECHO_PIN) == 1);
ça ne fait rien en attendant un signal sur ECHO_PIN : est-ce bien ce que tu veux ?
Je regarderai plus en détail lorsqu'il sera bien indenté...