Не понимаю проблемы с моим кодом

Я пытаюсь заставить машину-робота(elegoo one) пройти курс обхода препятствий. Я пытаюсь написать свой собственный с нуля, чтобы улучшить себя, и поэтому был бы очень признателен, если бы кто-то сказал мне, что не так с моим кодом и почему он не работает

#include <Servo.h>  //библиотека сервоприводов
Servo myservo; // создание объекта servo для управления серво
Servo servosweep; 
int Echo = A4;  
int Trig = A5; 

#define ENA 5
#define ENB 6
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11
#define carSpeedForward 100
#define carSpeedLeftandRight 150
int rightDistance = 0, leftDistance = 0, middleDistance = 0; 
int pos=0;


void forward(){ 
  analogWrite(ENA, carSpeedForward);
  analogWrite(ENB, carSpeedForward);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  Serial.println("Forward");
}

void back() {
  analogWrite(ENA, carSpeedForward);
  analogWrite(ENB, carSpeedForward);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  Serial.println("Back");
}

void left() {
  analogWrite(ENA, carSpeedLeftandRight);
  analogWrite(ENB, carSpeedLeftandRight);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH); 
  Serial.println("Left");
}

void right() {
  analogWrite(ENA, carSpeedLeftandRight);
  analogWrite(ENB, carSpeedLeftandRight);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  Serial.println("Right");
}

void stop() {
  digitalWrite(ENA, LOW);
  digitalWrite(ENB, LOW);
  Serial.println("Stop!");
} 

//Ultrasonic distance measurement Sub function
int Distance_test() {
  digitalWrite(Trig, LOW);   
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);  
  delayMicroseconds(20);
  digitalWrite(Trig, LOW);   
  float Fdistance = pulseIn(Echo, HIGH);  
  Fdistance= Fdistance / 58;       
  return (int)Fdistance;
}  

void setup() { 
  myservo.write(pos);
  myservo.attach(3);  // прикрепить сервопривод на выводе 3 к объекту сервопривода
  Serial.begin(9600);     
  pinMode(Echo, INPUT);    
  pinMode(Trig, OUTPUT);  
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);
  //stop();
} 

void loop(){
  for (pos = 0; pos < 90; pos += 1.0) {
  servosweep.write(pos);
  delay(50);
  leftDistance=Distance_test();
  }
  
  myservo.write(90);
  middleDistance=Distance_test();
  
  for (pos=90;pos<=180;pos+=1.0){
    servosweep.write(pos);
    delay(50);
    rightDistance=Distance_test();
    
  }

  
  if(rightDistance > leftDistance) {
        right();
        delay(360);
      }
      else if(rightDistance < leftDistance) {
        left();
        delay(360);
      }
      else if((rightDistance <= 40) || (leftDistance <= 40)) {
        back();
        delay(180);
      }
      else {
        forward();
      }
}

Я пытаюсь заставить ультразвуковой датчик считывать значения слева(от 0 до 90), посередине(90) и справа(от 90 до 180), а затем использовать их для сравнения, чтобы решить, куда идти. Когда я запускаю этот код, моя машина ездит по кругу, и я не знаю, в чем проблема. Любая помощь будет оценена по достоинству.