проблема связана с ардуино

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

#include <Wire.h>
//Библиотека связи I2C для MPU6050
#include <I2Cdev.h>
//Библиотека интерфейса MPU6050
#include <MPU6050_6Axis_MotionApps20.h>
//Обработка входящих последовательных данных
#include <CmdMessenger.h>
//Содержит определение максимальных пределов различных типов данных
#include <limits.h>

//Создание объекта MPU6050
MPU6050 mpu(0x68);
//Объект мессенджера
CmdMessenger Messenger_Handler = CmdMessenger(Serial);

/////////////////////////////////////////////////// //////////////////////////////////////
//Параметры DMP
//Устанавливаем true, если инициализация DMP прошла успешно
bool dmpReady = false;
//Содержит фактический байт состояния прерывания от MPU
uint8_t mpuIntStatus;
//возвращаем статус после каждой операции с устройством
uint8_t devStatus;
//Ожидаемый размер пакета DMP
uint16_t packetSize;
//подсчет всех байтов, находящихся в данный момент в FIFO
uint16_t fifoCount;
//буфер хранения FIFO
uint8_t fifoBuffer[64];


#define OUTPUT_READABLE_QUATERNION
/////////////////////////////////////////////////// ///////////////////////////////////////////////

// ориентация/переменная движения
Quaternion q;
VectorInt16 aa;
VectorInt16 aaReal;
VectorInt16 aaWorld;
VectorFloat gravity;

//Контейнер углов Эйлера
float euler[3];

//контейнер рыскания/тангажа/крена и вектор силы тяжести
float ypr[3];




// ============================================== ================
// === ПРОЦЕДУРА ОБНАРУЖЕНИЯ ПРЕРЫВАНИЯ ===
// ============================================== ================

volatile bool mpuInterrupt = false;     // указывает, перешел ли вывод прерывания MPU в высокий уровень
void dmpDataReady() {
    mpuInterrupt = true;
}

/////////////////////////////////////////////////// ///////////////
//Определение контакта двигателя
//Левые контакты двигателя

#define A_1 3
#define B_1 4

//Номер контакта ШИМ 1
#define PWM_1 6


//Правый мотор
#define A_2 4 
#define B_2 5

//Номер контакта ШИМ 2
#define PWM_2 5

/////////////////////////////////////////////////// /////////////////////////////////////////////////// //////////////////////////////////////////////////
//Определение ультразвуковых контактов
const int echo = 24, Trig = 23;
long duration, cm;

/////////////////////////////////////////////////// /////////////////////////////////////////////////// //////////////////////////////////////////////////

/////////////////////////////////////////////////// //////////////////////////////////////
//Скорость двигателя с ПК
// Скорость двигателя влево и вправо
float motor_left_speed = 0;
float motor_right_speed = 0;
/////////////////////////////////////////////////// ///////////////

//Настройка последовательного порта, энкодеров, ультразвука, MPU6050 и функций сброса
void setup()
{

  //Инициализируем последовательный порт со скоростью 115200 бод
  Serial.begin(115200);  

  //Настройка двигателей
  SetupMotors();
  //Настройка ультразвука
  SetupUltrasonic();  
  //Настройка MPU 6050
  Setup_MPU6050();

  //Настраиваем Мессенджер
// Messenger_Handler.attach(OnMssageCompleted);

}

void SetupMotors()
{

 //Левый мотор
 pinMode(A_1,OUTPUT);
 pinMode(B_1,OUTPUT); 


 //Правый мотор
 pinMode(A_2,OUTPUT);
 pinMode(B_2,OUTPUT);  

}


/////////////////////////////////////////////////// /////////////////////////////////////////////////// /////////////////////////////////////////////////// ///////////////////////////////////
//Настройка функции UltrasonicsSensor()
void SetupUltrasonic()
{
 pinMode(Trig, OUTPUT);
 pinMode(echo, INPUT); 

}

/////////////////////////////////////////////////// /////////////////////////////////////////////////// /////////////////////////////////////////////////// ///////////////////////////////////
//Настройка функции MPU6050

void Setup_MPU6050()
{


    Wire.begin();
   // инициализируем устройство
    Serial.println("Initializing I2C devices...");
    mpu.initialize();

    // проверяем соединение
    Serial.println("Testing device connections...");
    Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

    //Инициализируем DMP в MPU 6050
    Setup_MPU6050_DMP();


}
/////////////////////////////////////////////////// /////////////////////////////////////////////////// /////////////////////////////////////////////////// ///////////////////////////////////
//Настройка MPU 6050 DMP
void Setup_MPU6050_DMP()
{

     //Инициализация DMP

   devStatus = mpu.dmpInitialize();

   mpu.setXGyroOffset(220);
   mpu.setXGyroOffset(76);
   mpu.setXGyroOffset(-85); 
   mpu.setXGyroOffset(1788);  


   if(devStatus == 0){
         Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);
    //mpu.setDMPEnabled(истина);

// pinMode(PUSH2,INPUT_PULLUP);
// AttachInterrupt(PUSH2, dmpDataReady, RISING);

    mpuIntStatus = mpu.getIntStatus();

    dmpReady = true;

    packetSize = mpu.dmpGetFIFOPacketSize();

   }else{

     ;
     }   
}


void loop() {

 //Чтение из последовательного порта
    Read_From_Serial();

    //Отправляем ультразвуковые значения через последовательный порт
    Update_Ultra_Sonic();

    //Отправляем значения MPU 6050 через последовательный порт
    Update_MPU6050();


}

/////////////////////////////////////////////////// /////////////////////////////////////////////////// /////////////////////////////////////////////////// //////////////////////////////////
//Чтение из последовательной функции

void Read_From_Serial()

{
   while(Serial.available() > 0)
    {

       int data = Serial.read();
// Messenger_Handler.process(данные);


    } 

}

void Update_Ultra_Sonic()
{
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
  // Вывод эха используется для чтения сигнала от PING))): ВЫСОКИЙ
  // импульс, длительность которого равна времени (в микросекундах) с момента отправки
  // от пинга до получения его эха от объекта.
  duration = pulseIn(echo, HIGH);


  //Отправка через последовательный порт
  Serial.print("u");
  Serial.print("\t");
  Serial.print(cm);
  Serial.print("\n");

}

/////////////////////////////////////////////////// /////////////////////////////////////////////////// /////////////////////////////////////////////////// /////
//Обновление MPU6050

void Update_MPU6050()
{



  int16_t ax, ay, az;
  int16_t gx, gy, gz;

    ///Обновляем значения из DMP для получения вектора вращения
    Update_MPU6050_DMP();


}

/////////////////////////////////////////////////// /////////////////////////////////////////////////// ////////////////////////////////////////////////
//Обновление функций DMP MPU6050

void Update_MPU6050_DMP()
{

 //Обработка DMP
 // если программирование не удалось, не пытайтесь ничего делать
    if (!dmpReady) return;

     // ждем прерывания MPU или доступности дополнительных пакетов
    while (!mpuInterrupt && fifoCount < packetSize) {

        ;    

          }


// сбрасываем флаг прерывания и получаем байт INT_STATUS
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // получаем текущий счетчик FIFO
    fifoCount = mpu.getFIFOCount();

    // проверка на переполнение (это никогда не должно происходить, если только наш код не слишком неэффективен)
    if ((mpuIntStatus & 0x10) || fifoCount > 512) {
        // сброс, чтобы мы могли продолжить работу без ошибок
        mpu.resetFIFO();
    }



// в противном случае проверяем прерывание готовности данных DMP (это должно происходить часто)
else if (mpuIntStatus & 0x02) {
        // ожидание правильной доступной длины данных, ожидание должно быть ОЧЕНЬ коротким
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // читаем пакет из FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);

        // отслеживаем здесь счетчик FIFO, если есть > в наличии 1 пакет
        // (это позволяет нам сразу читать дальше, не дожидаясь прерывания)
        fifoCount -= packetSize;

        #ifdef OUTPUT_READABLE_QUATERNION
            // отображаем значения кватернионов в простой матричной форме: wxyz
            mpu.dmpGetQuaternion(&q, fifoBuffer);


            Serial.print("i");Serial.print("\t");
            Serial.print(q.x); Serial.print("\t");
            Serial.print(q.y); Serial.print("\t");
            Serial.print(q.z); Serial.print("\t");
            Serial.print(q.w);
            Serial.print("\n");


        #endif

        #ifdef OUTPUT_READABLE_EULER
            // отображаем углы Эйлера в градусах
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // отображаем углы Эйлера в градусах
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_REALACCEL
            // отображаем реальное ускорение, скорректированное для устранения гравитации
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            Serial.print("areal\t");
            Serial.print(aaReal.x);
            Serial.print("\t");
            Serial.print(aaReal.y);
            Serial.print("\t");
            Serial.println(aaReal.z);
        #endif

        #ifdef OUTPUT_READABLE_WORLDACCEL
            // отображаем начальное ускорение мировой рамки, скорректированное для устранения гравитации
            // и повернут на основе известной ориентации кватерниона
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
            Serial.print("aworld\t");
            Serial.print(aaWorld.x);
            Serial.print("\t");
            Serial.print(aaWorld.y);
            Serial.print("\t");
            Serial.println(aaWorld.z);
        #endif

        #ifdef OUTPUT_TEAPOT
            // отображаем значения кватернионов в демонстрационном формате InvenSense Teapot:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3] = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5] = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7] = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9] = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++; // packageCount, намеренно зацикливается на 0xFF
        #endif


    }


}