Почему мой код Arduino для запуска робота, управляемого Bluetooth, с датчиком препятствий не работает должным образом?

Ранее я создал робота, управляемого смартфоном на основе Bluetooth (4-колесный автомобиль), используя Arduino и робота-обходчика препятствий. Теперь я хотел объединить их обоих. Поэтому я объединил их функции и коды таким образом, чтобы это показалось мне правильным. Но моему роботу это кажется неправильным. Мой робот, управляемый Bluetooth, работал бесперебойно. Мой робот, избегающий препятствий, работал безупречно. Но когда я попытался присоединиться к ним, они оба заплакали.

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

Мне нужно загрузить весь код, потому что я не уверен, где я делаю это неправильно

#include <SoftwareSerial.h> 

#include <RemoteXY.h> 

// Настройки подключения RemoteXY  
#define REMOTEXY_SERIAL_RX A2 
#define REMOTEXY_SERIAL_TX A3 
#define REMOTEXY_SERIAL_SPEED 9600 


// Конфигурация RemoteXY   
#pragma pack(push, 1) 
uint8_t RemoteXY_CONF[] = 
  { 255,8,0,54,0,176,0,8,228,4,
  5,48,44,26,30,30,0,31,8,1,
  6,0,-84,-142,20,20,0,2,26,129,
  0,6,37,23,8,0,64,78,97,118,
  101,100,0,131,3,51,1,20,5,1,
  2,31,82,111,98,111,116,32,67,97,
  114,0,131,0,62,5,19,5,2,2,
  31,67,111,110,116,114,111,108,108,101,
  114,0,129,0,10,45,13,7,0,16,
  84,72,69,0,129,0,5,52,25,8,
  0,136,83,104,101,105,107,104,0,1,
  0,-34,-111,12,12,1,2,31,88,0,
  1,4,79,44,12,12,0,37,151,240,
  159,147,162,0,65,4,87,9,7,7,
  0,65,1,87,17,7,7,0,65,2,
  87,25,7,7,0,67,5,3,3,25,
  14,0,94,24,51,2,0,86,1,11,
  5,0,135,26,31,31,79,78,0,79,
  70,70,0 }; 

// эта структура определяет все переменные вашей
struct { 

    // входная переменная
  int8_t joystick_1_x; // =-100..100 x-координатное положение
  int8_t joystick_1_y; // =-100..100 y-координатное положение джойстика
  uint8_t rgb_1_r; // = 0..255 Значение красного цвета
  uint8_t rgb_1_g; // = 0..255 значение зеленого цвета
  uint8_t rgb_1_b; // = 0..255 Значение
  uint8_t button_1; // = 1, если кнопка нажата, else = 0
  uint8_t button_2; // = 1, если кнопка нажата, else = 0
  uint8_t switch_1; // = 1, если переключатель включен и = 0, если ВЫКЛЮЧЕН 

    // выходная переменная
  uint8_t red_led_r; // =0..255 LED Red brightness
  uint8_t blue_led_b; // =0..255 LED Blue brightness
  uint8_t green_led_g; // =0..255 LED Green brightness
  char text_indicator[51];  // строка UTF8 end zero 

    // другая переменная
  uint8_t connect_flag;  // =1 если провод подключен, else =0 

} RemoteXY; 
#pragma pack(pop) 

///////////////////////////////////////////// 
// END RemoteXY включает в себя          // 
///////////////////////////////////////////// 

#define PIN_BUTTON_2 A4
#define PIN_SWITCH_1 A5



#include<AFMotor.h>
#include <NewPing.h>
#include <Servo.h> 

#define TRIG_PIN A0 
#define ECHO_PIN A1 
#define MAX_DISTANCE 250
//#define MAX_SPEED 150 // устанавливает скорость вращения двигателей постоянного тока
//#определить MAX_SPEED_OFFSET 20

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 

AF_DCMotor left_motor_A(1, MOTOR12_64KHZ); 
AF_DCMotor left_motor_B(2, MOTOR12_64KHZ);
AF_DCMotor right_motor_A(3, MOTOR34_64KHZ);
AF_DCMotor right_motor_B(4, MOTOR34_64KHZ);
Servo myservo;   

int distance = 100;

int right_motor_speed = 0;
int left_motor_speed = 0;
// определить два массива со списком контактов для каждого двигателя
AF_DCMotor RightMotor[2] = {right_motor_A, right_motor_B};
AF_DCMotor LeftMotor[2] = {left_motor_A, left_motor_B};


//управление скоростью двигателей
void Wheel (AF_DCMotor * motor, int v) // v = скорость двигателя, двигатель = указатель на массив контактов 
{
  if (v > 100) v=100;
  if (v < -100) v=-100;
  if (v > 0){

    motor[0].run(FORWARD);
    motor[1].run(FORWARD);
    motor[0].setSpeed(v * 1.75);
    motor[1].setSpeed(v * 1.75);
  }
  else if ( v<0 ){

    motor[0].run(BACKWARD);
    motor[1].run(BACKWARD);
    motor[0].setSpeed(v * 1.75);
    motor[1].setSpeed(v * 1.75);
/*    //digitalWrite (двигатель [1], ВПЕРЕД);
    analogWrite (motor [2], (v) * 0.75);
    //Аналоговая запись (двигатель [2], (-в) * 0,75); */
  }
  else{
    motor[0].run(RELEASE);
    motor[1].run(RELEASE);
    motor[0].setSpeed(0);
    motor[1].setSpeed(0);
  }
}

 int lookRight()
 {
    RemoteXY.blue_led_b = 255;
    RemoteXY.red_led_r = 0;
    RemoteXY.green_led_g = 0;
    sprintf(RemoteXY.text_indicator, "CHECKING THE RIGHT SIDE.");
    myservo.write(50); 
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(115); 
    return distance;
 }

 int lookLeft()
 {
    RemoteXY.blue_led_b = 255;
    RemoteXY.red_led_r = 0;
    RemoteXY.green_led_g = 0;
    sprintf(RemoteXY.text_indicator, "CHECKING THE LEFT SIDE.");
    myservo.write(170); 
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(115); 
    return distance;
    delay(100);
 }

 int readPing() 
 { 
  delay(100);
  int cm = sonar.ping_cm();
   if(cm==0)
  {
    cm = MAX_DISTANCE ;
  } 
  return cm;
 }

 void moveStop() 
 {
  left_motor_A.run(RELEASE); 
  left_motor_B.run(RELEASE);
  right_motor_A.run(RELEASE);
  right_motor_B.run(RELEASE);

 } 



void setup()  
{ 
  RemoteXY_Init ();  

  pinMode (PIN_BUTTON_2, OUTPUT);
  pinMode (PIN_SWITCH_1, OUTPUT);

  myservo.attach(10);  
  myservo.write(115); 
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);


} 

void loop()  
{  
  RemoteXY_Handler (); 

  digitalWrite(PIN_BUTTON_2, (RemoteXY.button_2==0)?LOW:HIGH);
  digitalWrite(PIN_SWITCH_1, (RemoteXY.switch_1==0)?LOW:HIGH);

    int distanceR = 0;
    int distanceL =  0;
    delay(40);

    if(distance<=27)
    {
      moveStop();
      RemoteXY.blue_led_b = 0;
      RemoteXY.red_led_r = 255;
      RemoteXY.green_led_g = 0;
      sprintf(RemoteXY.text_indicator, "AN OBSTACLE HAS COME IN FRONT OF YOUR ROBOT!!!");
      digitalWrite(PIN_BUTTON_2, HIGH);
      delay(1500);
      distanceR = lookRight();
      delay(250);
      distanceL = lookLeft();
      delay(250);

      if(distanceR>=distanceL)
      {
        RemoteXY.blue_led_b = 255;
        RemoteXY.red_led_r = 0;
        RemoteXY.green_led_g = 0;

        sprintf(RemoteXY.text_indicator, "IT IS GOOD TO GO ON THE RIGHT SIDE.");
      }

      else
      {
        sprintf(RemoteXY.text_indicator, "IT IS GOOD TO GO ON THE LEFT SIDE.");
      } 
    }
    else
    {
      RemoteXY.blue_led_b = 0;
      RemoteXY.red_led_r = 0;
      RemoteXY.green_led_g = 255;
      //управление правым двигателем
      Wheel (LeftMotor, RemoteXY.joystick_1_y - RemoteXY.joystick_1_x);
      Wheel (RightMotor, RemoteXY.joystick_1_y + RemoteXY.joystick_1_x);

    }
    distance = readPing();

    //управление правым двигателем
    //Колесо (левый двигатель, RemoteXY.joystick_1_y - RemoteXY.joystick_1_x);
    //Колесо (rightMotor, RemoteXY.joystick_1_y + RemoteXY.joystick_1_x);


  }

Я хочу, чтобы мой робот работал по моим указаниям, и когда он почувствует что-то на своем переднем конце, он должен остановиться там и сказать мне через RemoteXY.text_string, а затем проверить наличие препятствий слева и справа и сказать мне, с какой стороны он доступен. После этого он снова должен принимать мои заказы через мой мобильный телефон.

Во-первых, стало невозможно подключить робота к смартфону по Bluetooth, потому что робот был очень занят какими-то другими делами (только он знает, что он делал). Затем я применил "Кнопку RemoteXY" к своему коду (которого нет в коде, который я публикую здесь) с блоком "if"; так что если этот переключатель будет нажат через смартфон, только тогда робот начнет двигаться. Итак, это помогло мне подключить моего робота к смартфону через Bluetooth, но когда я подключался, серводвигатель регулярно вращался (опять же, я не знаю почему) после безопасного соединения, когда я включил этот переключатель, мой робот принял только мой первый заказ (который обычно двигаясь вперед), и он выполнял мой первый заказ почти первые 3-5 секунд, а затем Робот завис, и мой смартфон автоматически отключился от робота.

Я не могу понять, где грязь в моем коде или мой код сам по себе полностью грязен. Это тот вопрос, где мне нужна ваша помощь, ребята

Другая информация, которая вам может понадобиться для лучшей поддержки / помощи, я использую: 1). Приложение RemoteXY для Android для общения роботов 2). Arduino UNO как микроконтроллер 3). Arduino IDE для программирования Arduino 4). Один серводвигатель SG 90. 5). Четыре редукторных двигателя постоянного тока. 6). Экран водителя мотора Л293Д для управлять моторами ДК и сервоприводом 7). Ультразвуковой датчик HC SR-04 для обнаружения препятствий 8). Батарея 12V 9). Модуль HC-05 для связи Bluetooth 10). Зуммер для внешнего индикатора / рупора. На контакт A4 11). Грязный код для программирования Arduino .

Любая помощь с кодом будет оценена по достоинству.

Заранее большое вам спасибо. ❤️