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");
}