Из двух логически сетчей эскизов один работает, а другой нет.

В настоящее время я работаю над проектом, включающим самобалансирующегося бота & нашел этот код в Интернете:

#include "PID_v1.h"
#include "LMotorController.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

#define MIN_ABS_SPEED 20

MPU6050 mpu;

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

// переменные ориентации/движения
Quaternion q;           // [w, x, y, z] контейнер кватернионов
VectorFloat gravity;    // [x, y, z] вектор гравитации
float ypr[3];           // [рыскание, тангаж, крен] контейнер рыскания/тангажа/крена и вектор силы тяжести

//ПИД
double originalSetpoint = 178.57;
double setpoint = 178.57;
double movingAngleOffset = 0.1;
double input, output;
double Kp = 50;
double Kd = 1.4;
double Ki = 60;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

double motorSpeedFactorLeft = 0.6;
double motorSpeedFactorRight = 0.5;
//КОНТРОЛЛЕР ДВИГАТЕЛЯ
int ENA = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 8;
int IN4 = 9;
int ENB = 10;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

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


void setup()
{
    // подключаемся к шине I2C (библиотека I2Cdev не делает этого автоматически)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 24; // Тактовая частота I2C 400 кГц (200 кГц, если процессор 8 МГц)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    // инициализируем последовательную связь
    // (115200 выбрано, потому что оно необходимо для вывода Teapot Demo, но это
    // действительно зависит от вас, в зависимости от вашего проекта)
    Serial.begin(115200);
    while (!Serial); // ждем перечисления Леонардо, остальные продолжают немедленно

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

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

    // загружаем и настраиваем DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // укажите здесь свои собственные смещения гироскопа, масштабированные по минимальной чувствительности
    mpu.setXGyroOffset(262);
    mpu.setYGyroOffset(-29);
    mpu.setZGyroOffset(-10);
    mpu.setXAccelOffset(-2109);
    mpu.setYAccelOffset(394);
    mpu.setZAccelOffset(1341); // 1688 заводских настроек для моего тестового чипа

    // убеждаемся, что это сработало (в этом случае возвращается 0)
    if (devStatus == 0)
    {
        // включаем DMP, теперь когда он готов
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // включаем обнаружение прерываний Arduino
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // устанавливаем флаг готовности DMP, чтобы основная функция цикла() знала, что его можно использовать
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // получаем ожидаемый размер пакета DMP для последующего сравнения
        packetSize = mpu.dmpGetFIFOPacketSize();

        //настройка ПИД

        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(10);
        pid.SetOutputLimits(-255, 255);  
    }
    else
    {
        // ОШИБКА!
        // 1 = первоначальная загрузка памяти не удалась
        // 2 = обновление конфигурации DMP не удалось
        // (если он сломается, обычно код будет 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }
}


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

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

        pid.Compute();
        motorController.move(output, MIN_ABS_SPEED);

    }

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

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

    // проверка на переполнение (это никогда не должно происходить, если только наш код не слишком неэффективен)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    {
        // сброс, чтобы мы могли продолжить работу без ошибок
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

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

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

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

        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        #ifdef LOG_INPUT
            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
        input = ypr[1] * 180/M_PI + 180;
        Serial.println(input);
   }
}

Репозиторий связан с https://github.com/kurimawxx00/arduino-self-balancing-robot

Мой код следующий:

    /* For documentation purposes
 *
 * #define MPU6050_INTERRUPT_FF_BIT            7
 * #define MPU6050_INTERRUPT_MOT_BIT           6
 * #define MPU6050_INTERRUPT_ZMOT_BIT          5
 * #define MPU6050_INTERRUPT_FIFO_OFLOW_BIT    4
 * #define MPU6050_INTERRUPT_I2C_MST_INT_BIT   3
 * #define MPU6050_INTERRUPT_PLL_RDY_INT_BIT   2
 * #define MPU6050_INTERRUPT_DMP_INT_BIT       1
 * #define MPU6050_INTERRUPT_DATA_RDY_BIT      0
 *
  */

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "PID_v1.h"
#include "LMotorController.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include <Wire.h>
#endif

#define MIN_SPEED 20

#define DEBUG_BUILD

MPU6050 mpu;

// Переменные для состояния и управления MPU
bool isDMPReady = false;
uint8_t dmpStatus; // 0, если инициализация прошла успешно
uint16_t dmpPacketSize; // по умолчанию = 42
uint16_t fifoCount;
uint8_t fifoBuffer[64];
uint8_t mpuIntStatus;

// Переменные для расчетных данных из DMP
Quaternion q; // // содержит QUAT_W, QUAT_X, QUAT_Y, QUAT_Z из FIFO
int16_t gyro[3]; // Удерживает вращение
VectorFloat gravity; // содержит вектор гравитации, рассчитанный по DMP
float ypr[3]; // 0 = рыскание, 1 = наклон, 2 = крен

// данные ПИД
double setPoint = 178.57;
double input = setPoint, output;
double Kp = 50;
double Kd = 1.4;
double Ki = 60;
PID pid(&input, &output, &setPoint, Kp, Ki, Kd, DIRECT);

// Конфигурация двигателя
const int inA = 6;
const int inB = 7;
const int inC = 8;
const int inD = 9;
const int enA = 5;
const int enB = 10;
const double motorCorrectionA = 0.6;
const double motorCorrectionB = 0.5;
LMotorController motorController(inA, inB, inC, inD, enA, enB, motorCorrectionA, motorCorrectionB);

// ============================================== "="
// === Процедура обработки прерывания ===
// ============================================== "="
volatile bool mpuInterrupt = false;
void dmpDataReady() {
    mpuInterrupt = true;
}

// ============================================== "="
// === Настройка программы ===
// ============================================== "="
void setup() {
    // подключаемся к шине I2C
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 24; // Тактовая частота I2C 400 кГц (200 кГц, если процессор 8 МГц)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    Serial.begin(115200);
    while(!Serial);

    // Инициализируем МПУ
    Serial.println(F("Initializing I2C Devices: "));
    mpu.initialize();
    dmpStatus = mpu.dmpInitialize();

    // Смещение гироскопа и ускорения
    mpu.setXGyroOffset(262);
    mpu.setYGyroOffset(-29);
    mpu.setZGyroOffset(-10);
    mpu.setXAccelOffset(-2109);
    mpu.setYAccelOffset(394);
    mpu.setZAccelOffset(1341);

    // Проверка инициализации
    #ifdef DEBUG_BUILD
        Serial.println(F("Testing MPU Status: "));
        Serial.println(mpu.testConnection() ? F("MPU6050 initialization success!") : F("MPU6050 initialization failed!"));
        Serial.println(dmpStatus == 0 ? F("DMP initialization successful!") : F("DMP initialization failed!"));
    #endif

    if(dmpStatus == 0){
        // Включаем DMP
        mpu.setDMPEnabled(true);

        attachInterrupt(0, dmpDataReady, RISING);

        // Получаем байт прерывания
        mpuIntStatus = mpu.getIntStatus();

        // Получаем размер пакета DMP; по умолчанию = 42Б
        dmpPacketSize = mpu.dmpGetFIFOPacketSize();

        // Проверка инициализации DMP
        #ifdef DEBUG_BUILD
            Serial.println(mpu.getFIFOEnabled() ? F("FIFO Enabled!") : F("FIFO Not Enabled!"));
            Serial.println(mpu.getXGyroFIFOEnabled() ? F("FIFO X Gyro Enabled!") : F("FIFO X Gyro Not Enabled!"));
            Serial.println(mpu.getYGyroFIFOEnabled() ? F("FIFO Y Gyro Enabled!") : F("FIFO Y Gyro Not Enabled!"));
            Serial.println(mpu.getZGyroFIFOEnabled() ? F("FIFO Z Gyro Enabled!") : F("FIFO Z Gyro Not Enabled!"));
            Serial.println(mpu.getAccelFIFOEnabled() ? F("FIFO Accel Enabled!") : F("FIFO Accel Not Enabled!"));
            Serial.print(F("DMP Packet Size: "));
            Serial.println(dmpPacketSize);
        #endif

        isDMPReady = true;

        // Настройка ПИД
        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(10);
        pid.SetOutputLimits(-160, 160);
    }
    else{
        Serial.println(F("DMP Error!!!"));
    }
}

// ============================================== "="
// === Цикл основной программы ===
// ============================================== "="
void loop() {
    // Ничего не делать, если инициализация DMP не удалась
    if(!isDMPReady) return;

    while(!mpuInterrupt && fifoCount < dmpPacketSize){
        pid.Compute();
        motorController.move(output, MIN_SPEED);
    }

    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    fifoCount = mpu.getFIFOCount();

    if(mpuIntStatus & 0x10 || fifoCount == 1024){
        mpu.resetFIFO();
        Serial.println("FIFO Overflow");
    }
    else if(mpuIntStatus & 0x02){
        while(fifoCount < dmpPacketSize) fifoCount = mpu.getFIFOCount();

        mpu.getFIFOBytes(fifoBuffer, dmpPacketSize);
        fifoCount -= dmpPacketSize;

        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        // TODO: реализовать дополнительный фильтр
        input = ypr[1] * 180 / M_PI + 180; // Предположим, что в вертикальном положении угол наклона составляет 180 градусов.

        #ifdef DEBUG_BUILD
            Serial.print("Yaw/Pitch/Roll: ");
            Serial.print(ypr[0] * RAD_TO_DEG);
            Serial.print("\t");
            Serial.print(ypr[1] * RAD_TO_DEG);
            Serial.print("\t");
            Serial.println(ypr[2] * RAD_TO_DEG);
        #endif
    }
}

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