Странный вывод ультразвукового датчика Arduino

Итак, я только что подключился к Arduino, купил комплект автомобильного робота Elegoo v3 и собрал его, но когда я поставил робота на «избегание объектов», он постоянно вращается, как будто перед ним находится объект. Я попытался изменить код, и мне не повезло, поэтому я подумал, что это может быть сам датчик, когда подключил его и запустил:

void loop() {
   a=sr04.Distance();
   Serial.print(a);
   Serial.println("cm");
   delay(1000);
}

Он отображал правильные расстояния, поэтому я решил, что это не может быть датчик. Любопытная вещь произошла, когда я добавил несколько строк в код автомобиля, чтобы он также печатал расстояние, которое он видел, и когда датчик подключен к машине по какой-либо причине, все, что он видит, составляет 18 см независимо от фактического расстояния, что объясняет, почему он постоянно вращается. Я нашел проблему, но теперь моя проблема в том, что я не могу понять, почему он постоянно определяет только 18 см, когда подключен к машине.

#include <SR04.h>

/*
 * @Description: Rocker_Control
 * @Author: HOU Changhua
 * @Date: 2019-08-12 18:00:25
 * @LastEditTime: 2019-08-27 10:45:29
 * @LastEditors: Please set LastEditors
 */
#include <Servo.h>
#include <stdio.h>
#include "HardwareSerial.h"
#include "ArduinoJson-v6.11.1.h" //ArduinoJson

/*Driving Interface for Ultrasound Ranging*/
#define ECHO_PIN A4
#define TRIG_PIN A5

/*Motor Drive Interface*/
#define ENA 5
#define ENB 6
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11

/*Driving Interface for Infrared Pipeline Patrol*/
#define LineTeacking_Pin_Right 10
#define LineTeacking_Pin_Middle 4
#define LineTeacking_Pin_Left 2
#define LineTeacking_Read_Right !digitalRead(10)
#define LineTeacking_Read_Middle !digitalRead(4)
#define LineTeacking_Read_Left !digitalRead(2)

#define carSpeed 180 //ШИМ (то есть: скорость двигателя/скорость автомобиля)

Servo servo;
unsigned long IR_PreMillis;
unsigned long LT_PreMillis;

enum FUNCTIONMODE
{
  IDLE,
  LineTeacking,
  ObstaclesAvoidance,
  Bluetooth,

} func_mode = IDLE; /*Functional model*/

enum MOTIONMODE
{
  STOP,
  FORWARD,
  BACK,
  LEFT,
  RIGHT
} mov_mode = STOP; /*Motion model*/

void delays(unsigned long t)
{

  for (unsigned long i = 0; i < t; i++)
  {
    getBTData_Plus();
    delay(1);
  }
}

/*Acquisition Distance: Ultrasound*/
unsigned int getDistance(void)
{
  static unsigned int tempda = 0;
  unsigned int tempda_x = 0;
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  tempda_x = ((unsigned int)pulseIn(ECHO_PIN, HIGH) / 58);
  if (tempda_x < 150)
  {
    tempda = tempda_x;
  }
  else
  {
    tempda = 30;
  }
  return tempda;
}
/*Control motor: */
void forward(bool debug, int16_t in_carSpeed)
{

  analogWrite(ENA, in_carSpeed);
  analogWrite(ENB, in_carSpeed);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  if (debug)
    Serial.println("Go forward!");
}

/*Control motor: */
void back(bool debug, int16_t in_carSpeed)
{

  analogWrite(ENA, in_carSpeed);
  analogWrite(ENB, in_carSpeed);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  if (debug)
    Serial.println("Go back!");
}
/*Control motor:*/
void left(bool debug, int16_t in_carSpeed)
{
  analogWrite(ENA, 250);
  analogWrite(ENB, 250);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  if (debug)
    Serial.println("Go left!");
}
/*Control motor:*/
void right(bool debug, int16_t in_carSpeed)
{
  analogWrite(ENA, 250);
  analogWrite(ENB, 250);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  if (debug)
    Serial.println("Go right!");
}
/*Control motor:*/
void stop(bool debug = false)
{
  digitalWrite(ENA, LOW);
  digitalWrite(ENB, LOW);
  if (debug)
    Serial.println("Stop!");
}
/*
  Bluetooth serial port data acquisition and control command parsing
*/
void getBTData_Plus(void)
{
  String comdata = "";

  while ((Serial.available() > 0) && (false == comdata.endsWith("}")))
  {
    comdata += char(Serial.read());
    delay(3);
  }
  if ((comdata.length() > 0) && (comdata != "") && (true == comdata.endsWith("}")))
  {
    StaticJsonDocument<200> doc;                                //Создаем объект JsonDocument
    DeserializationError error = deserializeJson(doc, comdata); //Десериализовать данные JSON
    if (!error)                                                 //Проверяем успешность десериализации
    {
      int control_mode_N = doc["N"];
      char buf[3];
      uint8_t temp = doc["H"];
      sprintf(buf, "%d", temp);
      String CommandSerialNumber = buf; //Получить серийный номер новой команды

      switch (control_mode_N)
      {
      case 5: /*Clear mode  processing <command：N 5>*/
        func_mode = Bluetooth;
        mov_mode = STOP;
        Serial.print('{' + CommandSerialNumber + "_ok}");
        break;
      case 3:               /*Remote switching mode  processing <command：N 3>*/
        if (1 == doc["D1"]) // Режим сдвига строки
        {
          func_mode = LineTeacking;
          Serial.print('{' + CommandSerialNumber + "_ok}");
        }
        else if (2 == doc["D1"]) //Режим обхода препятствий
        {
          func_mode = ObstaclesAvoidance;
          Serial.print('{' + CommandSerialNumber + "_ok}");
        }
        break;
      case 2: /*Remote switching mode  processing <command：N 2>*/

        if (1 == doc["D1"])
        {
          func_mode = Bluetooth;
          mov_mode = LEFT;
          Serial.print('{' + CommandSerialNumber + "_ok}");
        }
        else if (2 == doc["D1"])
        {
          func_mode = Bluetooth;
          mov_mode = RIGHT;
          Serial.print('{' + CommandSerialNumber + "_ok}");
        }
        else if (3 == doc["D1"])
        {
          func_mode = Bluetooth;
          mov_mode = FORWARD;
          Serial.print('{' + CommandSerialNumber + "_ok}");
        }
        else if (4 == doc["D1"])
        {
          func_mode = Bluetooth;
          mov_mode = BACK;
          Serial.print('{' + CommandSerialNumber + "_ok}");
        }
        else if (5 == doc["D1"])
        {
          func_mode = Bluetooth;
          mov_mode = STOP;
          Serial.print('{' + CommandSerialNumber + "_ok}");
        }
        break;
      default:
        break;
      }
    }
  }
}

/*Bluetooth remote control mode*/
void bluetooth_mode()
{
  if (func_mode == Bluetooth)
  {
    switch (mov_mode)
    {
    case FORWARD:
      forward(false, carSpeed);
      break;
    case BACK:
      back(false, carSpeed);
      break;
    case LEFT:
      left(false, carSpeed);
      break;
    case RIGHT:
      right(false, carSpeed);
      break;
    case STOP:
      stop();
      break;
    default:
      break;
    }
  }
}
/*
  Line Teacking Mode
*/
void line_teacking_mode(void)
{
  if (func_mode == LineTeacking)
  {
    if (LineTeacking_Read_Middle)
    {                           //Обнаружение в средней инфракрасной трубке
      forward(false, carSpeed); //Управляющий двигатель: автомобиль движется вперед
      LT_PreMillis = millis();
    }
    else if (LineTeacking_Read_Right)
    {                         //Обнаружение в правой инфракрасной трубке
      right(false, carSpeed); //Управляющий мотор: машина движется вправо
      while (LineTeacking_Read_Right)
      {
        getBTData_Plus(); //Сбор данных Bluetooth
      }
      LT_PreMillis = millis();
    }
    else if (LineTeacking_Read_Left)
    {                        // Обнаружение в левой инфракрасной трубке
      left(false, carSpeed); //Управляющий мотор: автомобиль движется влево
      while (LineTeacking_Read_Left)
      {
        getBTData_Plus(); //Сбор данных Bluetooth
      }
      LT_PreMillis = millis();
    }
    else
    {
      if (millis() - LT_PreMillis > 150)
      {
        stop(); // Остановить управление двигателем: отключить режим привода двигателя
      }
    }
  }
}
/*f(x) int */
static boolean function_xxx(long xd, long sd, long ed) //f(x)
{
  if (sd <= xd && xd <= ed)
    return true;
  else
    return false;
}
/*Obstacle avoidance*/
void obstacles_avoidance_mode(void)
{
  static uint16_t ULTRASONIC_Get = 0;
  static unsigned long ULTRASONIC_time = 0;
  static boolean stateCar = false;
  static boolean CarED = false;
  static uint8_t switc_ctrl = 0x00;
  static boolean timestamp = true;
   Serial.print(A4);
   Serial.println("cm");
   delay(1000);

  if (func_mode == ObstaclesAvoidance)
  {
    servo.attach(3);
    if (millis() - ULTRASONIC_time > 100)
    {
      ULTRASONIC_Get = getDistance(); //Измерение расстояния до препятствия
      ULTRASONIC_time = millis();
      if (function_xxx(ULTRASONIC_Get, 0, 30)) //Если расстояние меньше Xсм препятствий
      {
        stateCar = true;
        stop(); //останавливаться
      }
      else
      {
        stateCar = false;
      }
    }
    if (false == CarED)
    {
      if (stateCar == true)
      {
        timestamp = true;
        CarED = true;
        switc_ctrl = 0x00;
        stop();          //останавливаться
        servo.write(30); //устанавливает положение сервопривода в соответствии со значением
        delays(500);
        if (function_xxx(getDistance(), 0, 25)) //Сколько см впереди есть препятствия?
        {
          switc_ctrl |= 0x01;
          //идти к
        }
        else
        {
          switc_ctrl &= (~0x01);
        }
        servo.write(150); //устанавливает положение сервопривода в соответствии со значением
        delays(500);
        if (function_xxx(getDistance(), 0, 25)) //Сколько см впереди есть препятствия?
        {
          switc_ctrl |= 0x02;
          //идти к
        }
        else
        {
          switc_ctrl &= (~0x02);
        }
        servo.write(90); // сообщаем сервоприводу перейти в позицию в переменной 30
        delays(500);
        if (function_xxx(getDistance(), 0, 25)) //Сколько см впереди есть препятствия?
        {
          switc_ctrl |= 0x04;
          //идти к
        }
        else
        {
          switc_ctrl &= (~0x04);
        }
      }
      else
      {
        forward(false, 180); //Управляющий двигатель: автомобиль движется вперед
        CarED = false;
      }
    }

    if (true == CarED)
    {
      // Получить процессорное время
      static unsigned long MotorRL_time;
      if (timestamp == true || millis() - MotorRL_time > 420)
      {
        timestamp = false;
        MotorRL_time = millis();
      }
      if (function_xxx((millis() - MotorRL_time), 0, 400))
      {
        switch (switc_ctrl)
        {
        case 0 ... 1:
          left(false, 150); //Управляющий двигатель: автомобиль движется вперед и влево
          break;
        case 2:
          right(false, 150); //Управляющий двигатель: автомобиль движется вперед и вправо
          break;
        case 3:
          forward(false, 150); //Управляющий двигатель: автомобиль движется вперед
          break;
        case 4 ... 5:
          left(false, 150); //Управляющий двигатель: автомобиль движется вперед и влево
          break;
        case 6:
          right(false, 100); //Управляющий двигатель: автомобиль движется вперед и вправо
          break;
        case 7:
          back(false, 150); //Управляющий двигатель: автомобиль движется назад
          break;
        }
      }
      else
      {
        CarED = false;
      }
    }
  }
  else
  {
    servo.detach();
    ULTRASONIC_Get = 0;
    ULTRASONIC_time = 0;
  }
}
void setup(void)
{
  Serial.begin(9600);         //инициализация
  servo.attach(3, 500, 2400); //500: 0 градусов 2400: 180 градусов
  servo.write(90);            // устанавливает положение сервопривода в соответствии с 90 (средним)

  pinMode(ECHO_PIN, INPUT); //Инициализация ультразвукового модуля
  pinMode(TRIG_PIN, OUTPUT);

  pinMode(IN1, OUTPUT); // Конфигурация моторизованного порта
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  pinMode(LineTeacking_Pin_Right, INPUT); // Конфигурация порта модуля инфракрасного слежения
  pinMode(LineTeacking_Pin_Middle, INPUT);
  pinMode(LineTeacking_Pin_Left, INPUT);
}

void loop(void)
{
  getBTData_Plus();           //Сбор данных Bluetooth
  bluetooth_mode();           //Удаленный режим Bluetooth
  line_teacking_mode();       //Режим линейного тикинга
  obstacles_avoidance_mode(); //Режим обхода препятствий
}

Новая отладка:

/*Acquisition Distance: Ultrasound*/
unsigned int getDistance(void)
{
  static unsigned int tempda = 0;
  unsigned int tempda_x = 0;
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  tempda_x = ((unsigned int)pulseIn(ECHO_PIN, HIGH) / 58);
  if (tempda_x < 150)
  {
    tempda = tempda_x;
  }
  else
  {
    tempda = 30;
  }
  Serial.print(tempda);
  Serial.println("cm");
  delay(1000);
  return tempda;
}