Удали все неиспользуемые переменные из этой программы:
#include <Wire.h>  

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <Arduino.h>
#include <EncButton.h>
#include <ServoSmooth.h>
#define AMOUNT 4

MPU6050 mpu; 
bool dmpReady = false;  // Set true if DMP init was successful
uint8_t mpuIntStatus;   // Holds actual interrupt status byte from MPU
uint8_t devStatus;      // Return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // Expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // Count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
int16_t rawValue[6];    // Raw sensor values for accelerometer and gyroscope for each axis: {ax, ay, az, gx, gy, gz}
Quaternion q;           // [w, x, y, z]         Quaternion container
VectorFloat gravity;    // [x, y, z]            Gravity vector
float ypr[3];           // [yaw, pitch, roll]   Yaw/pitch/roll container and gravity vector



float init_yaw, init_pitch;
int initialAngle = 0;
int lastPosYaw = 0;
int lastPosPitch = 0;
int lastPosServYaw_0 = 0;
int lastPosServYaw_1 = 0;
int lastPosServPitch_2 = 0;
int lastPosServPitch_3 = 0;

const int potPinYaw = A0; // пин для потенциометра (A0)
const int potPinPitch = A1; // пин для потенциометра (A1)
const int buttonPin = 2; // пин для кнопки
int intPin = 3;  // These can be changed, 2 and 3 are the Arduinos ext int pins
int myLed = 13; // Set up pin 13 led for toggling
int speed_servo = 70;
float accel_servo = 0.7;

int16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output
int16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output
int16_t magCount[3];    // Stores the 16-bit signed magnetometer sensor output

float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0};  // Factory mag calibration and mag bias
float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0};      // Bias corrections for gyro and accelerometer
float magBias[3] = {491.73, 619.62, 201.81}, magScale[3] = {1.06, 0.84, 1.15};

//mag bias [mG]: 
//491.73, 619.62, 201.81
//mag scale []: 
//1.06, 0.84, 1.15

uint32_t delt_t = 0; 
uint32_t count = 0; 

float pitch, yaw, roll;
float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 
//float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion
float GyroMeasError = PI * (40.0f / 180.0f);
float GyroMeasDrift = PI * (0.0f  / 180.0f);
float beta = 0.1; //sqrt(3.0f / 4.0f) * GyroMeasError;   // compute beta
//float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;   // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
#define Ki 0.0f
uint8_t Mmode = 0x02;        // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
uint32_t Now = 0;
float deltat = 0.0f;
uint32_t lastUpdate = 0, firstUpdate = 0;
int i = 0;
int inPosPitch = 0 ;
int inPosYaw = 0;

float eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method


int init_0 = 92;
int init_1 = 69;
int init_2 = 116;
int init_3 = 95;

enum Ascale {
  AFS_2G = 0,
  AFS_4G,
  AFS_8G,
  AFS_16G
};

enum Gscale {
  GFS_250DPS = 0,
  GFS_500DPS,
  GFS_1000DPS,
  GFS_2000DPS
};

enum Mscale {
  MFS_14BITS = 0, // 0.6 mG per LSB
  MFS_16BITS      // 0.15 mG per LSB
};
enum DeviceMode {
  mode_set,
  mode_compens
};
DeviceMode currentMode = mode_set; // Изначально в основном режиме

uint8_t Gscale = GFS_250DPS;
uint8_t Ascale = AFS_16G;
uint8_t Mscale = MFS_16BITS;

Button button(buttonPin);
ServoSmooth servos[AMOUNT];
volatile bool mpuInterrupt = false;  // Indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}


void setup() 
{
  Wire.begin();
//  TWBR = 12;  // 400 kbit/sec I2C speed
  Serial.begin(115200);
//  Serial.println("error_yaw, error_com");
  
  // Set up the interrupt pin, its set as active high, push-pull
  pinMode(intPin, INPUT);
  digitalWrite(intPin, LOW);
  pinMode(myLed, OUTPUT);
  digitalWrite(myLed, HIGH);
  

  Serial.println("Starting..");
  delay(800);
  
  // Initialize device
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();
//  compass.init();
  pinMode(intPin, INPUT);

  // Verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // Load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

 
  // Make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    
    // Calibrate and fine tune MPU6050 with the offsets set above
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    mpu.PrintActiveOffsets();
    
    // Turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // Enable Arduino interrupt detection
    Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
    Serial.print(digitalPinToInterrupt(intPin));
    Serial.println(F(")..."));
    attachInterrupt(digitalPinToInterrupt(intPin), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    // Set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // Get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
      // ERROR!
      // 1 = initial memory load failed
      // 2 = DMP configuration updates failed
      // (if it's going to break, usually the code will be 1)
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
  }


  


  
  servos[0].attach(5, init_0); //yaw_right
  servos[0].smoothStart();
  
  servos[2].attach(6, init_2); //pitch_right
  servos[2].smoothStart();
  
  servos[1].attach(11, init_1); //yaw_left 
  servos[1].smoothStart();
  
  servos[3].attach(10, init_3); // pitch left
  servos[3].smoothStart();
  
  
  
  servos[0].setAccel(accel_servo);
  servos[0].setSpeed(speed_servo);
  servos[0].setAutoDetach(false);  
  servos[1].setAccel(accel_servo);
  servos[1].setSpeed(speed_servo);
  servos[1].setAutoDetach(false);  
  servos[2].setAccel(accel_servo);
  servos[2].setSpeed(speed_servo);
  servos[2].setAutoDetach(false);
  servos[3].setAccel(accel_servo);
  servos[3].setSpeed(speed_servo);
  servos[3].setAutoDetach(false);
  
  
  currentMode = mode_set;
  inPosYaw =  map(analogRead(potPinYaw), 0, 900, 0, 180); 
  inPosPitch = map(analogRead(potPinPitch), 0, 900, 0, 180);
  

}
float aRes = 16.0/32768.0;
float gRes = 250.0/32768.0;
float mRes = 10.*4912./32760.0; 

void loop() {
  updateOrientation();
  
  button.tick();
  if (button.click()) {
    Serial.println(" Button clicked");
    if (currentMode == mode_set) {
      Serial.println("set mode compense");
      currentMode = mode_compens;
      
      lastPosServYaw_0 = servos[0].getCurrentDeg();
      lastPosServYaw_1 = servos[1].getCurrentDeg();
      lastPosYaw = yaw;
      lastPosServPitch_2 = servos[2].getCurrentDeg();
      lastPosServPitch_3 = servos[3].getCurrentDeg();
      lastPosPitch = pitch; 
       
      Serial.print("Initial yaw = ");
      Serial.print(yaw);
      Serial.print(" Initial pitch = ");
      Serial.println(pitch);    
//      lastPosPitch = servos[2].getCurrentDeg();
//      servos[0].setTargetDeg(lastPosYaw);
//      servos[1].setTargetDeg(lastPosPitch );
//      servos[2].setTargetDeg(lastPosYaw);
//      servos[3].setTargetDeg(lastPosPitch);
      
    }
    else {
      currentMode = mode_set;
      Serial.println("set mode settings");
    }
  }
  if (currentMode == mode_set) {
    if (abs(map(analogRead(potPinYaw), 0, 900, 0, 180) - inPosYaw) >= 10) 
    {    settings_servo_yaw();      }

    if (abs(map(analogRead(potPinPitch), 0, 900, 0, 180) - inPosPitch) >= 10) 
    {    settings_servo_pitch();    }    
    
  } 
  else {
    compensation();
  }
  delay(10);

//  updateOrientation();
//  Serial.print("Yaw, Pitch, Roll: ");
//  Serial.print(int(yaw+180));
//  Serial.print(", ");
//  Serial.print(int(pitch));
//  Serial.print(", ");
//  Serial.println(int(roll));      

}

int computePID(float input, float setpoint, float kp, float ki, float kd, float dt, int minOut, int maxOut) {
  float err = setpoint - input;
  static float integral = 0, prevErr = 0;
  integral = constrain(integral + (float)err * dt * ki, minOut, maxOut);
  float D = (err - prevErr) / dt;
  prevErr = err;
  return constrain(err * kp + integral + D * kd, minOut, maxOut);
}


void compensation(){
  int err = lastPosYaw - yaw;
//  int err = computePID(yaw, lastPosYaw,3.0, 0.0001, 0.0001, 10, -180, 180); 
  int new_yaw_0 = lastPosServYaw_0 + err; // + err
  int new_yaw_1 = lastPosServYaw_1 + err;
  servos[0].write(new_yaw_0);
  servos[1].write(new_yaw_1);
//  Serial.print("Error yaw = ");
//  Serial.println(err);

//  int err_pit = computePID(pitch, lastPosPitch, 1.0, 0.0001, 0.1, 5, -180, 180);
  int err_pit = lastPosPitch - pitch;
  int new_pitch_2 = lastPosServPitch_2 + err_pit;
  int new_pitch_3 = lastPosServPitch_3 + err_pit;
  servos[2].write(new_pitch_2);
  servos[3].write(new_pitch_3);
}

void settings_servo_yaw(){
//  for (byte i = 0; i < AMOUNT; i++) {
//      servos[i].tickManual();   // двигаем все сервы. Такой вариант эффективнее отдельных тиков
//    }
  servos[0].tick();
  servos[1].tick();
//  int newPosYaw = map(analogRead(potPinYaw), 0, 900, 30, 100); 
//  int newPosPitch = map(analogRead(potPinPitch), 0, 900, 130, 180); 
  int newPosYaw_0 = map(analogRead(potPinYaw), 0, 900, 30, 132); 
  int newPosYaw_1 = map(analogRead(potPinYaw), 0, 900, 20, 112); 
  servos[0].setTargetDeg(newPosYaw_0); //init_0 - newPosYaw - init_1
  servos[1].setTargetDeg(newPosYaw_1);
  
  //servoYaw_left.write(angle);
  //servoYaw_right.write(angle);
  //servoPitch_left.write(angle);
  //servoPitch_right.write(angle);
  delay(50);
}



void settings_servo_pitch(){
//  for (byte i = 0; i < AMOUNT; i++) {
//      servos[i].tickManual();   // двигаем все сервы. Такой вариант эффективнее отдельных тиков
//    }
  servos[2].tick();
  servos[3].tick();
//  int newPosYaw = map(analogRead(potPinYaw), 0, 900, 30, 100); 
//  int newPosPitch = map(analogRead(potPinPitch), 0, 900, 130, 180); 
  int newPosPitch_2 = map(analogRead(potPinPitch), 0, 900, 75, 155); 
  int newPosPitch_3 = map(analogRead(potPinPitch), 0, 900, 45, 145); 

  servos[2].setTargetDeg(newPosPitch_2); //init_2 - newPosPitch - init_3
  servos[3].setTargetDeg(newPosPitch_3 );
  delay(50);
}


void updateOrientation()
{
  if (!dmpReady) return;
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {

      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
      mpu.getMotion6(&rawValue[0], &rawValue[1], &rawValue[2], &rawValue[3], &rawValue[4], &rawValue[5]);
      ypr[0] = ypr[0] * 180 / M_PI;
      ypr[1] = ypr[1] * 180 / M_PI;
      ypr[2] = ypr[2] * 180 / M_PI;
  
      //int yawValue = map(heading, -90, 90, 0, 180);            
      int yawValue = map(ypr[0], -90, 90, 0, 180);  
      int pitchValue = map(ypr[1], -90, 90, 0, 180);            
      int rollValue = map(ypr[2], -90, 90, 0, 180);
      yaw = yawValue;
      roll = pitchValue;
      pitch = rollValue;
      Serial.print("Yaw = ");
      Serial.print(yaw);
      Serial.print("Roll = ");
      Serial.println(pitch);
  }
}