Возможный шум от двигателей

Я пытаюсь управлять самобалансирующимся автомобилем, но кажется, что MPU6050 загрязняется, когда мои двигатели работают на полной скорости. Я управляю своими двигателями с помощью модуля L298N и с помощью ШИМ-сигнала (пины 5, 6, 9 и 10 от ардуино). Я читал о развязке с помощью конденсаторов, но точно не знаю, как мне это сделать.

Это мой код. Это немного сумбурно (извините за это, просто я все еще не понимаю, что происходит), а также изображение моего проекта:

#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;

#define DEBUG
//#определить OFFSET_1


//#определить СТОП


// переменные управления/состояния 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 euler[3]; // [пси, тета, фи] Контейнер угла Эйлера

//ПИД
// удвоить originalSetpoint = 175,8;
#ifdef OFFSET_1
      double originalSetpoint = 225;
#else
      double originalSetpoint = 184;
#endif
    

double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output, output_left, output_right, error;
int moveState=0; //0 = баланс; 1 = спина; 2 = вперед
double Kp = 40;
double Kd = 0.05;
double Ki = 5;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

double motorSpeedFactorLeft = 1;
double motorSpeedFactorRight = 1;
double motorSpeedFactorForward = 1;
double motorSpeedFactorBackward = 1;

//КОНТРОЛЛЕР ДВИГАТЕЛЯ
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);

//таймеры
long time1Hz = 0;
long time5Hz = 0;

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


void setup()
{

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

  Serial.begin(115200);
  mpu.initialize();

  devStatus = mpu.dmpInitialize();

  // укажите здесь собственные смещения гироскопа, масштабированные для минимальной чувствительности

  #ifdef OFFSET_1
      mpu.setXGyroOffset(0);
      mpu.setYGyroOffset(0);
      mpu.setZGyroOffset(0);
      mpu.setZAccelOffset(16372);
      mpu.setYAccelOffset(5);
      mpu.setXAccelOffset(2);
    #else
      mpu.setXGyroOffset(-52);
      mpu.setYGyroOffset(-12);
      mpu.setZGyroOffset(53);
      mpu.setZAccelOffset(2420);
      mpu.setYAccelOffset(2881);
      mpu.setXAccelOffset(-784);
    #endif  

  // убедиться, что это сработало (возвращает 0, если это так)
  if (devStatus == 0)
  {

    //mpu.CalibrateAccel(6);
    //mpu.CalibrateGyro(6);
    // включаем DMP, теперь, когда он готов
    mpu.setDMPEnabled(true);

    // включить обнаружение прерываний Arduino
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    // устанавливаем наш флаг готовности DMP, чтобы основная функция loop() знала, что ее можно использовать
    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(")"));
  }

  
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);

  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);

  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  
}
void loop()
{
  #ifdef STOP
      return;
  #endif
  
  // если программирование не удалось, не пытайтесь ничего сделать
  if (!dmpReady) return;

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

    output = 255;
    output_left = output * motorSpeedFactorLeft;
    output_right = output * motorSpeedFactorRight;
    if(input > (setpoint))
    {
      output_left = output_left*motorSpeedFactorForward ;
      output_right = output_right* motorSpeedFactorForward;
      
      Move_Forward();  
    }
    else if(input < (setpoint))
    {
      output_left = output_left*motorSpeedFactorBackward ;
      output_right = output_right* motorSpeedFactorBackward;
      Move_Backward();
    }
    else
    {
      Stop();
    }                
  }
  // сброс флага прерывания и получение байта 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(euler, &q, &gravity);
    input = (euler[1] * 180 / M_PI + 180);
    error = input - setpoint;
// Serial.print("\tOutput: ");
// Serial.print(output);
    Serial.print("\tError: ");  
    Serial.println(error);
    
      
  }
}

void Move_Forward() // Код для вращения колеса вперед
{

  
  //******ПРАВИЛЬНО******//
      analogWrite(5, -1*output_right);
      analogWrite(6, 0);

  //******ОСТАВИЛ******//
      analogWrite(9, -1*output_left);
      analogWrite(10, 0);
      
      
      digitalWrite(11, HIGH);
      digitalWrite(12, LOW);
}


void Move_Backward() // Код для вращения колеса вперед
{
  //******ПРАВИЛЬНО******//
      analogWrite(5, LOW);
      analogWrite(6, 1*output_right);
      
  //******ОСТАВИЛ******//
      analogWrite(9, 0);
      analogWrite(10, 1*output_left);

      digitalWrite(11, LOW);  
      digitalWrite(12, HIGH);
}

void Stop() // Код для вращения колеса вперед
{
      analogWrite(11, LOW);  
      analogWrite(12, LOW);  
      analogWrite(5, LOW);
      analogWrite(6, LOW);

      digitalWrite(9, LOW);
      digitalWrite(10, LOW);
}