const int Echo    = 2;
const int Trigger = 3;
const int Links   = 4;
const int Rechts  = 5;
const int Fehler  = 6;

int Dauer;

float Distanz;

bool Start        =  true;
bool Hinderniss   = false;
bool LinksDrehen  = false;
bool RechtsDrehen = false;
bool UmDrehen     = false;

long unsigned int Drehen;

void setup() {
  pinMode(Echo,     INPUT);
  pinMode(Trigger, OUTPUT);
  pinMode(Links,   OUTPUT);
  pinMode(Rechts,  OUTPUT);
}

float DistanzCM() {
  digitalWrite(Trigger,  LOW);
  delayMicroseconds(2);
  digitalWrite(Trigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trigger,  LOW);
  Dauer = pulseIn(Echo, HIGH);
  return Dauer * 0.034 / 2;
}

void loop() {

  while(Start == true){
    Distanz = DistanzCM();

    if(Distanz <= 10){
      Start       = false;
      Hinderniss  =  true;
      LinksDrehen =  true;
      delay(2000);
      Drehen = millis();
    }else{
      digitalWrite(Links,  HIGH);
     digitalWrite(Rechts, HIGH);
      delay(5);
      digitalWrite(Links,   LOW);
      digitalWrite(Rechts,  LOW);
      delay(5);
    }
  }

  while(Hinderniss == true){
    while(LinksDrehen == true){
      digitalWrite(Links, HIGH);
      delay(5);
      digitalWrite(Links,  LOW);
      delay(5);

      if(millis() - Drehen >= 2505){
        Distanz = DistanzCM();

        if(Distanz <= 10){
          RechtsDrehen =  true;
          LinksDrehen  = false;
          delay(2000);
          Drehen = millis();
        }else{
          Start       =  true;
          Hinderniss  = false;
          LinksDrehen = false;
        }
      }
    }

    while(RechtsDrehen == true){
      digitalWrite(Rechts, HIGH);
      delay(5);
      digitalWrite(Rechts,  LOW);
      delay(5);

      if(millis() - Drehen >= 5010){
        Distanz = DistanzCM();

        if(Distanz <= 10){
          UmDrehen     =  true;
          RechtsDrehen = false;
          delay(2000);
          Drehen = millis();
        }else{
          Start        =  true;
          Hinderniss   = false;
          RechtsDrehen = false;
        }
      }
    }

    while(UmDrehen == true){
      digitalWrite(Rechts, HIGH);
      delay(5);
      digitalWrite(Rechts,  LOW);
      delay(5);

      if(millis() - Drehen >= 2505){
        delay(2000);
        Distanz = DistanzCM();

        if(Distanz <= 10){
          while(Distanz <= 10){
            digitalWrite(Fehler, HIGH);
            delay(250);
            digitalWrite(Fehler, LOW);
            delay(250);
            Distanz = DistanzCM();
          }
        }else{
          Start      =  true;
          Hinderniss = false;
          UmDrehen   = false;
        }
      }
    }
  }
}