Novato en apuros: Código de autito controlado por Arduino Nano, uso de while

[fraruto]

Buenas, el codigo anda pero lo que quiero hacer es agregarle un while para que vaya marcha atrás hasta que ya pueda ir marcha adelante, porque sino se queda avanzando y retrocediendo (bugueado en esa secuencia) y lo quiero arreglar. Lo e intentado hacer pero no hay caso, me cuesta mucho.

#include <LiquidCrystal.h>
#include <Servo.h>                                                     

LiquidCrystal lcd(10, 11, 9, 8, 7, 6);
Servo servoo;                                                          

int duracion;
int distancia;
int Flag;                                                             
int infradato;                                                         
unsigned long tiempoInicio;                                            
bool mensajeCambiado = false;                                        
int servoang = 0;                                                      

#define inf 2

#define Eco
#define Dist

#define Fleds 3
#define Cleds A5
#define Ind 13

#define buzzer A0

#define trigg 4
#define echo 5

#define servo 12

#define pap1 A1
#define pap2 A2
#define pap3 A3
#define pap4 A4

void setup() {

  pinMode(Fleds, OUTPUT);
  pinMode(Cleds, OUTPUT);
  pinMode(Ind, OUTPUT);

  pinMode(buzzer, OUTPUT);
  pinMode(trigg, OUTPUT);
  pinMode(echo, INPUT);

  pinMode(inf, INPUT);

  servoo.attach(servo);

  pinMode(pap1, OUTPUT);
  pinMode(pap2, OUTPUT);
  pinMode(pap3, OUTPUT);
  pinMode(pap4, OUTPUT);

  lcd.begin(16, 2);
  lcd.setCursor(3, 0);                                                
  lcd.print("KIT MOVIL");
  lcd.setCursor(0, 1);
  lcd.print("Nombre mio");
  delay(3000);
  lcd.clear();

  servoo.write(90);                                                   
  delay(1000);

  tiempoInicio = millis();                                             

}

void loop() {

  int infradato = digitalRead(inf);

  digitalWrite(trigg, LOW);
  digitalWrite(trigg, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigg, LOW);
  duracion = pulseIn(echo, HIGH);
  distancia = duracion / 58.2;

  servoo.write(servoang);                                             

  if (infradato == LOW && distancia >= 10)                             
    Flag = 1;

  if (Flag == 1) {

    if (!mensajeCambiado) {                                           
      lcd.clear();
      lcd.setCursor(5, 0);
      lcd.print("Marcha");
      lcd.setCursor(4, 1);
      lcd.print("Adelante");
      mensajeCambiado = true;
      tiempoInicio = millis();

    }

    if (millis() - tiempoInicio >= 1000) {                             
      lcd.clear();
      lcd.setCursor(4, 0);
      lcd.print("DISTANCIA:");
      lcd.setCursor(5, 1);
      lcd.print(distancia);
      lcd.setCursor(10, 1);
      lcd.print("CM");

    }

    digitalWrite(Fleds, HIGH);
    digitalWrite(Ind, LOW);
    digitalWrite(Cleds, LOW);
    digitalWrite(buzzer, LOW);


    digitalWrite(pap1, HIGH);
    digitalWrite(pap2, LOW);
    digitalWrite(pap3, LOW);
    digitalWrite(pap4, HIGH);
    delay(3);

    digitalWrite(pap1, LOW);
    digitalWrite(pap2, LOW);
    digitalWrite(pap3, HIGH);
    digitalWrite(pap4, HIGH);
    delay(3);

    digitalWrite(pap1, LOW);
    digitalWrite(pap2, HIGH);
    digitalWrite(pap3, HIGH);
    digitalWrite(pap4, LOW);
    delay(3);

    digitalWrite(pap1, HIGH);
    digitalWrite(pap2, HIGH);
    digitalWrite(pap3, LOW);
    digitalWrite(pap4, LOW);
    delay(3);

    servoang = 90;                                                   

  }

  if (infradato == HIGH || distancia < 10)                            
    Flag = 0;

  if (Flag == 0) {

    if (mensajeCambiado) {                                            
      lcd.clear();
      lcd.setCursor(5, 0);
      lcd.print("Marcha");
      lcd.setCursor(5, 1);
      lcd.print("Atras");
      mensajeCambiado = false;                                        
      tiempoInicio = millis();                                        

    }

    if (millis() - tiempoInicio >= 1000) {                            
      lcd.clear();
      lcd.setCursor(4, 0);
      lcd.print("DISTANCIA:");
      lcd.setCursor(5, 1);
      lcd.print(distancia);
      lcd.setCursor(10, 1);
      lcd.print("CM");

    }

    digitalWrite(Fleds, LOW);
    digitalWrite(Ind, HIGH);
    digitalWrite(Cleds, HIGH);
    digitalWrite(buzzer, HIGH);
    
    digitalWrite(pap1, HIGH);
    digitalWrite(pap2, HIGH);
    digitalWrite(pap3, LOW);
    digitalWrite(pap4, LOW);
    delay(3);

    digitalWrite(pap1, LOW);
    digitalWrite(pap2, HIGH);
    digitalWrite(pap3, HIGH);
    digitalWrite(pap4, LOW);
    delay(3);

    digitalWrite(pap1, LOW);
    digitalWrite(pap2, LOW);
    digitalWrite(pap3, HIGH);
    digitalWrite(pap4, HIGH);
    delay(3);

    digitalWrite(pap1, HIGH);
    digitalWrite(pap2, LOW);
    digitalWrite(pap3, LOW);
    digitalWrite(pap4, HIGH);
    delay(3);

    servoang = 25;             
                                           
  }
}

Les comento de qué va el código de forma resumida por si les cuesta entender:

Al inicio muestra mi nombre por 3s e inicializa el servo a 90°. Avanza midiendo la distancia y el suelo mostrando el mensaje marcha adelante por 1s para luego cambiar al de distancia y pone el servo a 90°. Lo mismo que lo anterior pero para marcha atrás pero con el servo a 25°.