Problem z MPU6050, zły odczyt

Question

Dzień dobry, 
Od razu zaznaczam że jest to mój drugi projekt który robię w swoim życiu. Wykonuje samo balansującego robota. No i wszystko dobrze gdyby nie to że MPU6050 źle odczytuje położenie. Np. Postawiłem robota prosto - MPU6050 wysyłała do Seriala zero - czyli tak jak powinno - jednak jeżeli przechylę o np. 5 stopni i wrócę do poprzedniego stanu to może być albo zero (jak powinno) albo np. 3. Nie zawsze to występuje jednak gdy robot często się przewraca to MPU6050 głupieje.

Biblioteki nie znam niestety, skopiowałem ją tydzień temu z jakiegoś filmiku bodajże a moja pamięć zawodzi.

Używam:
Arduino Nano
6v 4xAA
L293D
MPU6050

Mój kod:
 

#include <Wire.h>

//Variables
float elapsedTime, time, timePrev;        //Variables for time control
int gyro_error=0;                         //We use this variable to only calculate once the gyro data error
int16_t Gyr_rawX, Gyr_rawY, Gyr_rawZ;     //Here we store the raw data read 
float Gyro_angle_x, Gyro_angle_y;         //Here we store the angle value obtained with Gyro data
float Gyro_raw_error_x, Gyro_raw_error_y; //Here we store the initial gyro data error

int minustys;

/* Silniki */
const int leftBackward = 3;
const int leftForward = 4;
const int rightBackward = 5;
const int rightForward = 6;

bool tryb = true;

 
void setup(){
  Wire.begin();                           //begin the wire comunication  
  Wire.beginTransmission(0x68);           //begin, Send the slave adress (in this case 68)              
  Wire.write(0x6B);                       //make the reset (place a 0 into the 6B register)
  Wire.write(0x00);
  Wire.endTransmission(true);             //end the transmission
  
  Wire.beginTransmission(0x68);           //begin, Send the slave adress (in this case 68) 
  Wire.write(0x1B);                       //We want to write to the GYRO_CONFIG register (1B hex)
  Wire.write(0x10);                       //Set the register bits as 00010000 (1000dps full scale)
  Wire.endTransmission(true);             //End the transmission with the gyro

  Serial.begin(9600);                     //Remember to set this same baud rate to the serial monitor  
  time = millis();                        //Start counting time in milliseconds

/*Here we calculate the gyro data error before we start the loop
 * I make the mean of 200 values, that should be enough*/
  if(gyro_error==0)
  {
    for(int i=0; i<200; i++)
    {
      Wire.beginTransmission(0x68);            //begin, Send the slave adress (in this case 68) 
      Wire.write(0x43);                        //First adress of the Gyro data
      Wire.endTransmission(false);
      Wire.requestFrom(0x68,4,true);           //We ask for just 4 registers 
         
      Gyr_rawX=Wire.read()<<8|Wire.read();     //Once again we shif and sum
      Gyr_rawY=Wire.read()<<8|Wire.read();
   
      /*---X---*/
      Gyro_raw_error_x = Gyro_raw_error_x + (Gyr_rawX/32.8); 
      /*---Y---*/
      Gyro_raw_error_y = Gyro_raw_error_y + (Gyr_rawY/32.8);
      if(i==199)
      {
        Gyro_raw_error_x = Gyro_raw_error_x/200;
        Gyro_raw_error_y = Gyro_raw_error_y/200;
        gyro_error=1;
      }
    }
  }  

  minustys -= 1;
}
 
void loop(){
  timePrev = time;                        // the previous time is stored before the actual time read
    time = millis();                        // actual time read
    elapsedTime = (time - timePrev) / 1000; //divide by 1000 in order to obtain seconds

    Wire.beginTransmission(0x68);            //begin, Send the slave adress (in this case 68) 
    Wire.write(0x43);                        //First adress of the Gyro data
    Wire.endTransmission(false);
    Wire.requestFrom(0x68,4,true);           //We ask for just 4 registers
        
    Gyr_rawX=Wire.read()<<8|Wire.read();     //Once again we shif and sum
    Gyr_rawY=Wire.read()<<8|Wire.read();

    Gyr_rawX = (Gyr_rawX/32.8) - Gyro_raw_error_x; 
    Gyr_rawY = (Gyr_rawY/32.8) - Gyro_raw_error_y;
    
    Gyro_angle_x = Gyro_angle_x  + Gyr_rawX*elapsedTime;
    Gyro_angle_y = Gyro_angle_y  + Gyr_rawY*elapsedTime;
    Serial.print("GyroY angle: ");
    Serial.println(Gyro_angle_y);

    if(static_cast<int>(Gyro_angle_y)>3)
    {
      digitalWrite(leftBackward, HIGH);
      digitalWrite(leftForward, LOW);
      digitalWrite(rightForward, LOW); 
      digitalWrite(rightBackward, HIGH);
      Serial.print("1");
    }
    if (static_cast<int>(Gyro_angle_y)<minustys)
    {
      digitalWrite(leftBackward, LOW);
      digitalWrite(leftForward, HIGH);
      digitalWrite(rightForward, HIGH); 
      digitalWrite(rightBackward, LOW); 
      Serial.print("2");
    }
    if (static_cast<int>(Gyro_angle_y)>minustys && static_cast<int>(Gyro_angle_y)<3)
    {
      digitalWrite(leftForward, LOW);
      digitalWrite(rightForward, LOW);
      digitalWrite(leftBackward, LOW);
      digitalWrite(rightBackward, LOW); 
      Serial.print("3");
    }
}