Non ti garantisco il funzionamento....
#include <Servo.h>
Servo Testa;
const int triggerPort = 7; //uscita segnale trigger
const int echoPort = 8;//ingresso echo
const int servopin = 2;
const int servopin2 = 3;
int conta = 0;
void setup()
{
Testa.attach(4);
pinMode (servopin,OUTPUT);
pinMode (servopin2,OUTPUT);
pinMode( triggerPort, OUTPUT );// triggerPort è una porta in uscita
pinMode( echoPort, INPUT ); // echoPort è una porta in ingresso
Serial.begin( 9600 ); //imposta la porta seriale del monitor a 9600
Serial.println( "Sensore ultrasuoni: "); //scrive sul monitor la scritta tra ""
conta = 5;
}
void loop()
{
do {
cammina();
}
while (letturadistanza() < 500);
conta--;
if (conta==0) {
testaa();
conta = 5;
}
else {
cammina();
}
}
void cammina() {
for (int angle=72; angle<=90; angle++) servopulse(servopin, angle);
for (int angle=105; angle>=90; angle--) servopulse(servopin2, angle);
delay(200);
for (int angle=90; angle<=108; angle++) servopulse(servopin, angle);
for (int angle=90; angle>=75; angle--) servopulse(servopin2, angle);
delay(200);
for (int angle=108; angle>=90; angle--) servopulse(servopin, angle);
for (int angle=75; angle<=90; angle++) servopulse(servopin2, angle);
delay(200);
for (int angle=90; angle>=72; angle--) servopulse(servopin, angle);
for (int angle=90; angle<=105; angle++) servopulse(servopin2, angle);
delay(200);
}
void testaa(){
// per far riallineare l'insetto---------------------------------------
for (int angle=72; angle<=90; angle++) servopulse(servopin, angle);
for (int angle=105; angle>=90; angle--) servopulse(servopin2, angle);
delay(200);
//-----------------------------------qui sotto gira la testa------------------------------
for (int angle2=90; angle2>=0; angle2--){ //ciclo for che sposta l'angolo del servo
//Posizione = analogRead (Pot);
//Gradi=map(90);
Testa.write(angle2);
delay(30);
}
delay(2000);
//------------------------------
for (int angle2=0; angle2<=45; angle2++){ //ciclo for che sposta l'angolo del servo
Testa.write(angle2);
delay(30);
}
delay(2000);
//-----------------------------
for (int angle2=45; angle2<=90; angle2++){ //ciclo for che sposta l'angolo del servo
Testa.write(angle2);
delay(30);
}
delay(2000);
//-----------------------------
for (int angle2=90; angle2<=135; angle2++){ //ciclo for che sposta l'angolo del servo
Testa.write(angle2);
delay(30);
}
delay(2000);
//-----------------------------
for (int angle2=135; angle2<=180; angle2++){ //ciclo for che sposta l'angolo del servo
Testa.write(angle2);
delay(30);
}
delay(2000);
//-----------------------------
for (int angle2=180; angle2>=90; angle2--){ //ciclo for che sposta l'angolo del servo
Testa.write(angle2);
delay(30);
}
delay(2000);
}
unsigned long letturadistanza (){
digitalWrite( triggerPort, LOW ); //setta al livello logico 0 la porta trigger
//invia un impulso di 10microsec su trigger
digitalWrite( triggerPort, HIGH );//setta al livello logico 1 (5V) la porta trigger
delayMicroseconds( 10 ); //ritardo di 10 microsecondi
digitalWrite( triggerPort, LOW ); //setta al livello logico 0 la porta trigger
long duration = pulseIn( echoPort, HIGH ); //long variabili a 32bit, duration nome della variabile
// mette dentro duration il tempo che trascorre prima che echoPort diventa allo stato logico1
long r = 0.034 * duration / 2; //long variabili a 32bit, r nome della variabile
//mette dentro r
Serial.print( "durata: " ); //scrive sul monitor la scritta tra ""
Serial.print( duration ); //scrive sul monitor la la variabile tra()
Serial.print( " , " ); //scrive sul monitor la scritta tra ""
Serial.print( "distanza: " ); //scrive sul monitor la scritta tra ""
//dopo 38ms è fuori dalla portata del sensore, adesso fino a 50ms
if( duration > 50000 ) Serial.println( "fuori portata");
else {
Serial.print( r );
Serial.println( "cm" );
}
//aspetta 1.5 secondi
delay(500);
return duration;
}
void servopulse(int pin,int angle){
unsigned long pulsewidth = (angle*11) + 500;
digitalWrite (pin,HIGH);
delayMicroseconds (pulsewidth);
digitalWrite (pin,LOW);
delay (15);
}
Quando una funzione richiamata termina il programma prosegue dal comando successivo alla funzione.
Dimentica Goto, Pic e Visual Basic, smetti di saltare a destra e sinistra come una Rana e approfondisci i comandi e le strutture del C. 
Oltre al Do..While, con il controllo alla fine, c'è il While che controlla all'inizio del ciclo.
Ti ho inserito una funzione che restituisce un parametro ed ho usato variabili locali al posto di quelle globali.
Non c'è bisogno di scrivere due volte la funzione servopulse, basta passare in ingresso variabili diverse.
Dovresti ridurre al minimo l'uso dei delay.