Arduino (линейный последователь + ультразвуковой датчик + серводвигатель)

Я делаю машину-последователь линии, которая следует за линией при нажатии кнопки. И с помощью ультразвукового датчика я хочу, чтобы автомобиль остановился, когда расстояние до объекта впереди ближе 100, а затем заставил серводвигатель повернуться на 180 градусов.

Используется:

• Arduino UNO
• 2 Двигатель постоянного тока
• линейный датчик cytron maker
• 1 серводвигатель
• 1 ультразвуковой датчик
• 1 кнопка.

Застрял в кодировании, мне нужна помощь, пожалуйста.

int E1 = 4;
int M1 = 5; // Справа
int M2 = 6; // Слева
int E2 = 7;
int trigPin = 4;
int echoPin = 5;


void setup() {
  Serial.begin(9600);
  
  pinMode(E1, OUTPUT);
  pinMode(M1, OUTPUT);
  pinMode(E2, OUTPUT);
  pinMode(M2, OUTPUT);
  
  digitalWrite(E1, HIGH);
  digitalWrite(E2, HIGH);
  
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}


void loop() {
  float duration, distance;
  
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  duration = pulseIn(echoPin, HIGH);
  distance = ((float)(340 * duration) / 1000) / 2;
  
  Serial.print("\nDIstance:");
  Serial.print(distance);
  Serial.println("mm\n");
  delay(200);
  
  int D1 = digitalRead(8);
  int D2 = digitalRead(9);
  int D3 = digitalRead(10);
  int D4 = digitalRead(11);
  int D5 = digitalRead(12);
  
  if (D1 == 0 && D2 == 0 && D3 == 1 && D4 == 0 && D5 == 0) {
    analogWrite(M1, 100);
    analogWrite(M2, 100);
  }
  else if (D1 == 0 && D2 == 1 && D3 == 1 && D4 == 0 && D5 == 0) {
    analogWrite(M1, 80);
    analogWrite(M2, 100);
  }
  else if (D1 == 0 && D2 == 1 && D3 == 0 && D4 == 0 && D5 == 0) {
    analogWrite(M1, 60);
    analogWrite(M2, 100);
  }
  else if (D1 == 1 && D2 == 1 && D3 == 0 && D4 == 0 && D5 == 0) {
    analogWrite(M1, 30);
    analogWrite(M2, 100);
  }
  else if (D1 == 1 && D2 == 0 && D3 == 0 && D4 == 0 && D5 == 0) {
    analogWrite(M1, 0);
    analogWrite(M2, 100);
  }
  else if (D1 == 0 && D2 == 0 && D3 == 1 && D4 == 1 && D5 == 0) {
    analogWrite(M1, 100);
    analogWrite(M2, 80);
  }
  else if (D1 == 0 && D2 == 0 && D3 == 0 && D4 == 1 && D5 == 0) {
    analogWrite(M1, 100);
    analogWrite(M2, 60);
  }
  else if (D1 == 0 && D2 == 0 && D3 == 0 && D4 == 1 && D5 == 1) {
    analogWrite(M1, 100);
    analogWrite(M2, 30);
  }
  else if (D1 == 0 && D2 == 0 && D3 == 0 && D4 == 0 && D5 == 1) {
    analogWrite(M1, 100);
    analogWrite(M2, 0);
  }
  else if (D1 == 0 && D2 == 0 && D3 == 0 && D4 == 0 && D5 == 0) {
    analogWrite(M1, 0);
    analogWrite(M2, 0);
  }
  else if (D1 == 1 && D2 == 1 && D3 == 1 && D4 == 1 && D5 == 1) {
    analogWrite(M1, 00);
    analogWrite(M2, 00);
  }
  else {
  }
}