Как запрограммировать 2 ультразвуковых датчика, один на серводвигателе и один прикрепленный на передней стороне автомобиля?

Я хочу прикрепить два ультразвуковых датчика к препятствию, чтобы избежать столкновения с автомобилем.

1. Один на передней части машины
2. Во-вторых, на серводвигателе автомобиля

Это проект по преодолению препятствий. Я получаю проблему с кодом.

 **Во-первых, код переднего датчика получает выполнение, в то время как второй выключен. И второй датчик включается, когда первый выключен, И снова второй выключается, и первый включается. И цикл продолжается. Я хочу запустить оба датчика параллельно друг другу, то есть оба датчика должны работать вместе.** 

Вот мой код:

#include <Servo.h> //Библиотека серводвигателей
#include <NewPing.h> //Библиотека функций ультразвукового датчика

const int LeftForward = 7;// L298N управляющие контакты
const int LeftBackward = 6;
const int RightForward = 9;
const int RightBackward = 8;
int ENA = 10;
int ENB = 5;
#define trig_pin1 A1 //контакты датчика - аналоговый вход 1
#define echo_pin1 A2 //аналоговый вход 2
#define trig_pin2 A3 //контакты датчика - аналоговый вход 1
#define echo_pin2 A4
#define maximum_distance 200

boolean goesForward = false;
int distance = 100;
long duration;
NewPing sonar1(trig_pin1, echo_pin1,  maximum_distance); //функция датчика
Servo servo_motor; //servo name

void setup(){
  pinMode(RightForward, OUTPUT);
  pinMode(LeftForward, OUTPUT);
  pinMode(LeftBackward, OUTPUT);
  pinMode(RightBackward, OUTPUT);
  pinMode(trig_pin2, OUTPUT); 
  pinMode(echo_pin2, INPUT);

  analogWrite(ENA, 255);    //установите максимальную скорость
  analogWrite(ENB, 255);
  servo_motor.attach(11); //servo pin
  servo_motor.write(115);
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
}

void loop(){
  digitalWrite(trig_pin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin2, HIGH);
  delayMicroseconds(2);
  duration = pulseIn(echo_pin2, HIGH);
  distance = duration/58.2;
  delay(10);
  if(distance >19)
         {digitalWrite(RightForward, HIGH);
          digitalWrite(RightBackward, LOW);
          digitalWrite(LeftForward, HIGH);                                
          digitalWrite(LeftBackward, LOW);                                                       
          }
  if (distance < 18)
  {
    digitalWrite(RightForward, LOW);  //Stop                
    digitalWrite(RightBackward, LOW);
    digitalWrite(LeftForward, LOW);                                
    digitalWrite(LeftBackward, LOW);
    delay(500);
    digitalWrite(RightForward, LOW);      //movebackword         
    digitalWrite(RightBackward, HIGH);
    digitalWrite(LeftForward, LOW);                                
    digitalWrite(LeftBackward, HIGH);
    delay(500);
    digitalWrite(RightForward, LOW);  //Stop                
    digitalWrite(RightBackward, LOW);
    digitalWrite(LeftForward, LOW);                                
    digitalWrite(LeftBackward, LOW);  
    delay(100);  
    digitalWrite(RightForward, HIGH);       
    digitalWrite(RightBackward, LOW);   
    digitalWrite(LeftForward, LOW);                                 
    digitalWrite(LeftBackward, LOW);  
    delay(500);
  }





  int distanceRight = 0;
  int distanceLeft = 0;

  delay(50);
  if (distance <= 40){
    moveStop(); // препятствие, вероятно, находится на пути вперед, поэтому остановитесь
    delay(300);
    moveBackward();
    delay(400);
    moveStop();
    delay(300);
    distanceRight = lookRight();
    delay(300);
    distanceLeft = lookLeft();
    delay(300);
    if (distance >= distanceLeft){
      turnRight(); // вычислить, в каком направлении препятствие больше
      moveStop();
    }
    else{
      turnLeft();
      moveStop();
    }
  }
  else{
    moveForward(); 
  }
    distance = readPing();
}

int lookRight(){  
  servo_motor.write(50);
  delay(500);
  int distance = readPing();
  delay(100);
  servo_motor.write(115);
  return distance;
}

int lookLeft(){
  servo_motor.write(170);
  delay(500);
  int distance = readPing();
  delay(100);
  servo_motor.write(115);
  return distance;
  delay(100);
}

int readPing(){
delay(70);
int cm = sonar1.ping_cm();
if (cm==0){
    cm=250;
  }
  return cm;
}

void moveStop(){
  digitalWrite(RightForward, LOW);
  digitalWrite(LeftForward, LOW);
  digitalWrite(RightBackward, LOW);
  digitalWrite(LeftBackward, LOW);
}

void moveForward(){
  if(!goesForward){
    goesForward=true;
    digitalWrite(LeftForward, HIGH);
    digitalWrite(RightForward, HIGH);
    digitalWrite(LeftBackward, LOW);
    digitalWrite(RightBackward, LOW); 
  }
}

void moveBackward(){
  goesForward=false;
  digitalWrite(LeftBackward, HIGH);
  digitalWrite(RightBackward, HIGH);
  digitalWrite(LeftForward, LOW);
  digitalWrite(RightForward, LOW);
}

void turnRight(){
  digitalWrite(LeftForward, HIGH);
  digitalWrite(RightBackward, HIGH);
  digitalWrite(LeftBackward, LOW);
  digitalWrite(RightForward, LOW);
  delay(500);
  digitalWrite(LeftForward, HIGH);
  digitalWrite(RightForward, HIGH);
  digitalWrite(LeftBackward, LOW);
  digitalWrite(RightBackward, LOW);
}

void turnLeft(){
  digitalWrite(LeftBackward, HIGH);
  digitalWrite(RightForward, HIGH);
  digitalWrite(LeftForward, LOW);
  digitalWrite(RightBackward, LOW);
  delay(500);
  digitalWrite(LeftForward, HIGH);
  digitalWrite(RightForward, HIGH);
  digitalWrite(LeftBackward, LOW);
  digitalWrite(RightBackward, LOW);
}

что плохого я делаю в том, что не заставляю оба датчика работать вместе?