Ayuda con codigo para robot con ultra sonido

hola como estan?, queria ver si me pueden ayudar con un aspecto de mi codigo, estoy armando un robot recoge basura con motores DC, servos y un sensor de ultra sonido, tengo este codigo armado que al estar a una distancia menor a 20cm ejecuta unas acciones para agarrar la basura con una pala y una escoba hechas en 3d, mi problema viene a que no se como hacer para si por ejemplo detecta basura a 100,110,120cm, etc se mueva hasta que llegue a 20cm y ejecute la accion, basicamente que no importa la distancia en la que este mientras lo detecte se mueva hasta llegar a los 20cm y mueva los servos
adjunto codigo:

#include <Servo.h>
#include <Ultrasonic.h>

Ultrasonic ultrasonic(6, 7); // (Trig PIN,Echo PIN)
Servo miniservo1;
Servo miniservo2;

int distancia;

unsigned long tiempo1 = 0;
unsigned long tiempo2 = 0;
unsigned long tiempoSegundos = 0;

bool flag = true;

int IN3 = 5;      // IN1 de L298N a pin digital 2
int IN4 = 4;      // IN2 de L298N a pin digital 3
int ENB = 3;      // ENA de L298N a pin digital 5

int IN2 = 8;      // IN1 de L298N a pin digital 2
int IN1 = 9;      // IN2 de L298N a pin digital 3
int ENA = 10;      // ENA de L298N a pin digital 5


void setup() {
  miniservo1.attach(12);
  miniservo2.attach(11);

  pinMode(IN3, OUTPUT);   // pin 2 como salida
  pinMode(IN4, OUTPUT);   // pin 3 como salida  
  pinMode(ENB, OUTPUT);   // pin 5 como salida

  pinMode(IN1, OUTPUT);   // pin 2 como salida
  pinMode(IN2, OUTPUT);   // pin 3 como salida  
  pinMode(ENA, OUTPUT);   // pin 5 como salida

  // coloca el servo en su posición central
  miniservo1.write(90);
  miniservo2.write(20);
  tiempo1 = millis();
}

void loop() {

  distancia = ultrasonic.Ranging(CM); //Lectura sensor de ultrasonidos

  if (distancia <= 20) {
    if (flag) {
      tiempo1 = millis();
      flag = false;
    }

    if (millis() - tiempo1 >= 3000UL) {  //si la distancia es menor de 20...
      tiempo1 = millis(); //Actualiza el tiempo actual
      //tiempoSegundos = tiempo1/5000;
      //movemos el servo en una dirección
  digitalWrite(ENB, HIGH);  // habilita motor A (giro en un sentido)
  digitalWrite(IN3, LOW); // IN1 a 0
  digitalWrite(IN4, HIGH);  // IN2 a 1

  digitalWrite(ENA, HIGH);  // habilita motor A (giro en un sentido)
  digitalWrite(IN2, LOW); // IN1 a 0
  digitalWrite(IN1, HIGH);  // IN2 a 1

      delay(5000);
  digitalWrite(ENB, LOW); // ENA en 0 para deshabilitar motor
  digitalWrite(ENA, LOW); // ENA en 0 para deshabilitar motor
      delay(2000);

      miniservo1.write(0);
      delay(4000);
      miniservo2.write(0);
      delay(4000);
      miniservo2.write(100);
      delay(4000);
      
  digitalWrite(ENB, HIGH);  // habilita motor A (giro en sentido contrario)
  digitalWrite(IN3, HIGH);  // IN1 a 1
  digitalWrite(IN4, LOW); // IN2 a 0

  digitalWrite(ENA, HIGH);  // habilita motor A (giro en sentido contrario)
  digitalWrite(IN2, HIGH);  // IN1 a 1
  digitalWrite(IN1, LOW); // IN2 a 0
  
      delay(2000);

  digitalWrite(ENB, LOW); // ENA en 0 para deshabilitar motor
  digitalWrite(ENA, LOW); // ENA en 0 para deshabilitar motor
    }

  } else { //si no hay nada, siguen en la posición inicial
    miniservo1.write(60);
    miniservo2.write(110);

  digitalWrite(ENB, LOW); // ENA en 0 para deshabilitar motor
  digitalWrite(ENA, LOW); // ENA en 0 para deshabilitar motor

    flag = true;
  }
  delay(500);
}

desde ya muchas gracias. saludos!

Moderador:
Por favor, lee las Normas del foro y edita tu titulo usando uno que no contenga palabras inútiles como Ayuda.
Lee el punto 5. de las normas.