MPU6050

[Stephanie]

  tengo un gyroscopio MPU6050 conectado a mi arduino uno, este funciona bn, sin embargo... el punto de referencia, a medida que muevo el gyroscopio y lo regreso a su pocision inicial, varía hasta un punto en el que llega a ser muy diferente del inicial... es un problema del programa, sin embargo no se como hacer... no lo entiendo... aqui adjunto el programa... porfa ayuda!!


#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Servo.h>

Servo servo1;
Servo servo2;

MPU6050 accelgyro;

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

//Usado para calcular o angulo - Variaveis do acelerometro
double accXangle;
double accYangle;
double accZangle;
//Usado para calcular o angulo - Variaveis do giroscopio
double gyroXangle = 180;
double gyroYangle = 180;
double gyroZangle = 180;
int antx, anty;
uint32_t timer;

void setup() {
 
  Wire.begin();

  // Inicializando a comunicação serial
  // funciona em 8MHz ou em 16MHz
  Serial.begin(38400);

  // Iniciando dispositivos
  Serial.println("Inicializando cominicação I2C...");
 
  accelgyro.initialize();

  // Testando a conexão com a MPU6050
  Serial.println("Testando a conexão com MPU6050...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 conectada com sucesso" : "Falha na conexão com a MPU6050");
 
  servo1.attach(3);
  servo2.attach(5);
   
  timer = micros();
}

void loop() {
  // Fazendo a leitura de conexão com a MPU6050
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
/*
  // Calcular os angulos com base nos sensores do acelerometro
  accXangle = (atan2(ax,az) + PI) * RAD_TO_DEG;
  accYangle = (atan2(ay,az) + PI) * RAD_TO_DEG;
  accZangle = (atan2(ax,ay) + PI) * RAD_TO_DEG; 
*/
//  double gyroXrate = ((double)gx / 131.0);
  double gyroYrate = -((double)gy / 131.0);
  double gyroZrate = -((double)gz / 131.0);
 
  //###################### Calcular el ângulo de giro del gyroscopio
  gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
  gyroZangle += gyroZrate*((double)(micros()-timer)/1000000);
 if(abs(gyroZangle - antx) <= 1)
 {gyroZangle =antx;}
  if(abs(gyroYangle - anty) <= 1)
 {gyroYangle =anty;}
  servo1.write(int(gyroYangle));
  servo2.write(int(gyroZangle-100));
antx = gyroZangle;
anty = gyroYangle;
  timer = micros();
  //Angulo Giroscopio x/y/z
  Serial.print(int(gyroYangle)); Serial.print("\t");
  Serial.print(int(gyroZangle-100)); Serial.print("\t");
    delay(80);   
  Serial.print("\n");
}