Arduino Roboter - Servo reagiert nicht korrekt

[Sisor]

Hallo wendtdir,
ich habe deinen Code mal ein wenig aufgeräumt (nicht vollständig). Vergleiche mal mit deinem Code.
Ich hoffe es fällt auf, dass der aufgeräumte Code deutlich lesbarer ist. Wenn es um Fehlersuche geht, ist Lesbarkeit ein sehr wichtiger Faktor. Da fallen dann Dinge ins Auge, die vorher im "Gewusel" untergegangen sind.

#include <Servo.h>

#define echoPin    7  
#define trigPin    6  
#define motorAPin 12
#define motorBPin 13
#define vAPin      3
#define vBPin     11
#define bremseAPin 9
#define bremseBPin 8
#define led1Pin    2
#define led2Pin    4
#define servoPin   5

Servo myservo;

void kopf_rechts(){
  myservo.write(35);
}

void kopf_links(){
  myservo.write(125);
}

void kopf_mitte(){
  //Servo 0-Position (etwas schief, daher 80°)
  myservo.write(80);
}

void geradeaus_fahren() {
  digitalWrite(motorAPin, HIGH);
  digitalWrite(motorBPin, HIGH);
  digitalWrite(bremseAPin, LOW);
  digitalWrite(bremseBPin, LOW);
  digitalWrite(led1Pin, HIGH);
  digitalWrite(led2Pin, LOW);
}

unsigned long ping_cm() {
  // Ultraschall-Messung
  // gibt Distanz in cm zurück
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2); 
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); 
  digitalWrite(trigPin, LOW);
  // maximale Pulslänge bei 3m Distanz,  v_Schall = 340m/s
  const unsigned long maxPulse_us = (3 * 2 * 1000000) / 340;  
  unsigned long duration = pulseIn(echoPin, HIGH, maxPulse_us);
  if (duration == 0) return 300;  //kein Echo -> Rückgabe max.Messweite
  return duration / 58.2;
}

void setup(){
  myservo.attach(servoPin);
  kopf_mitte();
 
  // Pins für Motorenrichtungen als Ausgang
  pinMode(motorAPin, OUTPUT);
  pinMode(motorBPin, OUTPUT);
  
  // Pins für Bremsen als Ausgang
  pinMode(bremseAPin, OUTPUT);
  pinMode(bremseBPin, OUTPUT);

  // Geschwindigkeiten voll setzen
  analogWrite(vAPin, 255);
  analogWrite(vBPin, 255);
  
  // Bremse an (Motoren aus)
  digitalWrite(bremseAPin, HIGH);
  digitalWrite(bremseBPin, HIGH);

  // 2 Sekunden warten nach einschalten
  delay(2000);

  //US-Sensor
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  //LEDs
  pinMode(led1Pin, OUTPUT);
  pinMode(led2Pin, OUTPUT);
}

void loop(){
  //Distanz mit Ultraschall-Sensor ermitteln
  long distance = ping_cm();
  
  //Distanz größer 10cm, fährt vorwärts
  if (distance > 10) {
    geradeaus_fahren();
  }
  else { //Distanz kleiner gleich 10cm
    // Bremse ein, LED wechsel und Kopf drehen.
    digitalWrite(bremseAPin, HIGH);
    digitalWrite(bremseBPin, HIGH);
    digitalWrite(led1Pin, LOW);
    digitalWrite(led2Pin, HIGH);

    kopf_links();   
    delay(500);
    long links_cm = ping_cm();

    kopf_rechts(); 
    delay(500);
    long rechts_cm = ping_cm();
      
    if (links_cm > 10 && rechts_cm <= 10){
      digitalWrite(motorAPin, HIGH);
      digitalWrite(motorAPin, LOW);
      digitalWrite(bremseAPin, LOW);
      digitalWrite(bremseBPin, LOW);
      delay(500);
      geradeaus_fahren();
    }
    else if (links_cm <= 10 && rechts_cm > 10){
      digitalWrite(motorAPin, LOW);
      digitalWrite(motorAPin, HIGH);
      digitalWrite(bremseAPin, LOW);
      digitalWrite(bremseBPin, LOW);
      delay(500);
      geradeaus_fahren();
    }

    kopf_mitte();
    delay(500);
  }
}