Programmazione motore evita ostacoli

Salve, mi sono appena iscritto quindi scusate se ho sbagliato sessione o simili…

ho un problema con arduino uno, vorrei fare un robot che evita gli ostacoli “guardando” con un sonar…
Il codice è questo…ma esegue solamente il “guarda a destra” e non tutte le altre azioni trovando l’ostacolo sia di fronte che a destra…
ecco il codice…

#include <Servo.h> // include la Libreria Servo.h
#define echoPort  10
#define triggerPort  9
#define motoreD 8 // led rosso
#define motoreS 7 // led blu
Servo myservo;  // crea l’oggetto di tipo Servo, myservo sarà l’oggetto su cui opererare           
void setup() 
{ 
  myservo.attach(6);
  // lega l’oggetto myservo al pin a cui abbiamo collegato il servo, in questo caso il pin 6
  pinMode(echoPort, INPUT);
  pinMode(triggerPort, OUTPUT);
  pinMode(motoreD, OUTPUT);
  pinMode(motoreS, OUTPUT);
  Serial.begin(9600);
}   
void loop() {
  //porta bassa l'uscita del trigger
digitalWrite( triggerPort, LOW );
//invia un impulso di 10microsec su trigger
digitalWrite( triggerPort, HIGH );
delayMicroseconds( 5 );
digitalWrite( triggerPort, LOW ); 
long durata = pulseIn( echoPort, HIGH ); 
long distanza = 0.034 * durata / 2;
Serial.print(distanza); 
Serial.println(" cm     ");

 myservo.write(90);//guarda avanti


 if(distanza < 25 and distanza != 0){myservo.write(50);//guarda a destra
                                      if(distanza > 25 and distanza != 0){ //controlla se ha la destra libera e se la ha gira a destra 
                                           digitalWrite(motoreD, LOW); 
                                           digitalWrite(motoreS, HIGH);
                                           delay(500);
                                           myservo.write(90); //guarda nuovamente dritto
                                           }} else if(distanza < 25 and distanza != 0) {//nel caso non ha la destra libera guarda a sinistra 
                                            myservo.write(130);
                                            if(distanza > 25 and distanza != 0){ //controlla se ha la sinistra libera e se la ha gira a sinistra
                                             digitalWrite(motoreD, HIGH); 
                                             digitalWrite(motoreS, LOW);
                                             delay(500);
                                             myservo.write(90); //guarda nuovamente dritto
                                           } 
        
}
//cammina dritto
 digitalWrite(motoreD, HIGH); 
 digitalWrite(motoreS, HIGH);
}

Dove ho sbagliato? Qualche consiglio per semplificare la cosa? Grazie a tutti anticipatamente =)

Prima di tutto, nella sezione in lingua Inglese si può scrivere SOLO in Inglese ... quindi, per favore, la prossima volta presta più attenzione, poi, essendo questo il tuo primo post, ti chiedo cortesemente di presentarti QUI (spiegando bene quali conoscenze hai di elettronica e di programmazione) e di leggere con attenzione il REGOLAMENTO ... Grazie.

Guglielmo

P.S.: Il tuo post è stato spostato nell'opportuna sezione del forum "Italiano"