Probleme mit If Abfragen

Hallo, ich bin ziemlich unerfahren mit dem Programmieren von Arduinos und hadere gerade an meinem Prgogramm für ein selbstfahrendes Auto. Das Auto hat zwei Motoren über die auch die Lenkung läuft. Zudem hat es einen Ultraschallsensor, der auf einem Servo montiert ist. In der Theorie sollte es, solange genügend Abstand ist, immer geradeaus fahren. Wird der Abstand unterschritten soll das Auto stehenbleiben und den Servo auf 5 verschiedene Winkel stellen und immer den Abstand messen. Die Werte werden in einem Array gespeichert und der größte Wert ermittelt. In die Richtung mit dem größten Wert soll es dann fahren und das ganze prozedere beginnt von neuem. Allerdings dreht sich der Servo irgendwie die ganze Zeit und es funktioniert nichts wie gewünscht, ich erkenne aber nicht woran es liegt. Könnte es sein das ich die If schleifen falsch anwende?

#include <Servo.h>
Servo servo;
byte echopin=6;
byte trigpin=7;
long entfernung;
long value180[5];
long valuegerade[1];
int mo1 = 2;
int mo11 = 3;
int mo2 = 4;
int mo22 = 5;
int i;
int max = value180[0];
int nm = 0;

void setup() {
  pinMode(trigpin, OUTPUT);
  pinMode(echopin, INPUT);
  pinMode(mo1, OUTPUT);
  pinMode(mo11, OUTPUT);
  pinMode(mo2, OUTPUT);
  pinMode(mo22, OUTPUT);
  Serial.begin(9600);
  servo.attach(9);
}

void loop() {
  messengerade();
  while(valuegerade[0]>30){
    motorgerade();
  }
  messen180();
  if(value180[2]<=30){
     for (i = 0; i<=4; ++i){
        if (value180[i] > max){
        nm = i;
        max = value180[i];      
        }
      }
        if(nm>2){
          int motordrehung = map(nm,3, 4, 127, 255);
          analogWrite(1, motordrehung);
          digitalWrite(11, LOW);
          digitalWrite(2, LOW);
          digitalWrite(22, LOW);
          delay(635);
        }
        if(nm<2){
          int motordrehung = map(nm, 0, 1, 127, 255);
          digitalWrite(1, LOW);
          digitalWrite(11, LOW);
          analogWrite(2, motordrehung);
          digitalWrite(22,LOW);
          delay(635);         
        }
        else{
          digitalWrite(11, LOW);
          digitalWrite(1, HIGH);
          digitalWrite(22, LOW);
          digitalWrite(2, HIGH);
          delay(500);
        }
    }
}

void messen180(){
for(int stelle=0; stelle<=4; stelle++){
    int winkel = map(stelle, 0, 4, 0, 180);
    servo.write(winkel);

    digitalWrite(trigpin,HIGH);
    delayMicroseconds(1000);
    digitalWrite(trigpin,LOW);
    entfernung=(pulseIn(echopin,HIGH)/2)*0.03432;
    value180[stelle]=entfernung;

    delay(200);
  }
}

void messengerade(){
    servo.write(90);

    digitalWrite(trigpin,HIGH);
    delayMicroseconds(1000);
    digitalWrite(trigpin,LOW);
    entfernung=(pulseIn(echopin,HIGH)/2)*0.03432;
    valuegerade[0]=entfernung;

    delay(200);
}

void motorgerade(){
    digitalWrite(mo11, HIGH);
    digitalWrite(mo1, LOW);
    digitalWrite(mo22, HIGH);
    digitalWrite(mo2, LOW);
}

void motorstop(){
    digitalWrite(mo11, LOW);
    digitalWrite(mo1, LOW);
    digitalWrite(mo22, LOW);
    digitalWrite(mo2, LOW);
}

Ihr Beitrag wurde an seinen aktuellen Speicherort verschoben, da er besser geeignet ist.

Es gibt keine if-Schleifen.
Die heißen "if-Abfragen".
Beschreibe doch bitte etwas besser, was funktioniert in deinem Sketch und was funktioniert nicht. Fehler oder Probleme findest du, wenn du serielle Ausgaben auf den "seriellen Monitor" einbaust.

Du hast einige Probleme mehr als nur die Bedingung.

void loop()
{
  messengerade();
  while (valuegerade[0] > 30)
  {
    motorgerade();
  }

Wenn beim Start messengerade() in valuegerade[0] einen Wert von z.B. 50 schreibt, ist die Bedingung:

  while (valuegerade[0] > 30)

Immer erfüllt, da Du in

void motorgerade()
{
  digitalWrite(mo11, HIGH);
  digitalWrite(mo1, LOW);
  digitalWrite(mo22, HIGH);
  digitalWrite(mo2, LOW);
}

keine Abbruchbedingung hast.
Das Teil würde immer motorgerade ausführen....

Nachtrag:

Ich hab da mal was umgebaut:im loop()


void loop()
{
  messengerade();
  if (valuegerade[0] > 30)
  {
    motorgerade();
  }
  else
  {
    motorstop();
    messen180();
    if (value180[2] <= 30)

Und die Funktion motorgerade:

void motorgerade()
{
  digitalWrite(mo11, HIGH);
  digitalWrite(mo1, LOW);
  digitalWrite(mo22, HIGH);
  digitalWrite(mo2, LOW);
  messengerade();
}

Jetzt musst Du weiter bauen.