Bon, je vais essayer de t'aider comme je peux 
Si j'ai bien compris ton code, ton capteur (HC-SR04 ?) est monté sur un servo qui fait des aller/retour de 0-180° comme un radar. A chaque degrés/position tu contrôles la distance en cm ?
J'espere que je me plante pas 
Bref, je te suggere 2 méthodes. Après avoir fais comme toi, le truc ultrason à la main, j'utilise au choix deux librairies :
- la NewPing : https://code.google.com/p/arduino-new-ping/
- Ou la UltraSonic : HCSR04Ultrasonic - TetraSys
Avec la Newping, il y a un exemple d'interuption qui peut être intéressant pour toi...
J'ai comme projet un truc similaire, mais j'ai pas encore les servos ni les moteurs =(
Mais je vais le coder comme ça :
En interruption : la rotation du servo+contrôle distance.
Le robot avance...
Si dans les positions autour de 90° du servo/ultrason (donc pratiquement tout droit) -> mode ralenti
Je regarde la ou il y a le plus de distance et je tourne jusqu'à ce que ma zone soit proche de 90°
On peut ajouter un mode urgence quand on est proche d'un mur avec une petite marche arrière.
Et on boucle...
Voilà en gros.
Je vais bientôt le faire, je te donnerai des news.
Bon courage !