mpu6050校正偏移植

rn9409

想問各位這程式如我要做mpu6050陀螺儀的校正該怎麼做?
分別對AX做/16384的動作，還是對gx做甚麼動作不太懂

#include <Wire.h>
#include <Servo.h>

Servo servo1;
Servo servo2;

#define servo1_center 92  
#define servo2_center 95
#define servo_max 150  
#define servo_min 30
int servo1_angle, servo2_angle;

//Direccion I2C de la IMU
#define MPU 0x68 //定義MPU的I2C地址

//Ratios de conversion//定意轉換率
#define A_R 16384.0
#define G_R 131.0

//Conversion de radianes a grados 180/PI 這邊是180/圓周率=180/3.14　將弧度轉為角度
#define RAD_A_DEG = 57.295779

//MPU-6050 da los valores en enteros de 16 bits
//Valores sin refinar
int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;
int val1;
int val2;
//Angulos
float Acc[2];
float Gy[2];
float Angle[2];


void setup()
{
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
servo1.attach(5);
servo2.attach(6);
servo1_angle = servo1_center;
servo2_angle = servo2_center;
servo1.write(servo1_angle);
servo2.write(servo2_angle);
Serial.begin(38400);

}

void loop()
{
   //valores del Acelerometro de la IMU
   Wire.beginTransmission(MPU);
   Wire.write(0x3B);  //Pedir el registro 0x3B - corresponde al AcX
   Wire.endTransmission(false);
   Wire.requestFrom(MPU,6,true); //A partir del 0x3B, se piden 6 registros
   AcX=Wire.read()<<8|Wire.read(); //Cada valor ocupa 2 registros
   AcY=Wire.read()<<8|Wire.read();
   AcZ=Wire.read()<<8|Wire.read();
Serial.print("AcZ "); Serial.print(AcZ); Serial.print("\n");
//angulos Y, X respectivamente
   Acc[1] = atan(-1*(AcX/A_R)/sqrt(pow((AcY/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
   Acc[0] = atan((AcY/A_R)/sqrt(pow((AcX/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;

//valores del Giroscopio
   Wire.beginTransmission(MPU);
   Wire.write(0x43);
   Wire.endTransmission(false);
   Wire.requestFrom(MPU,4,true);      //A diferencia del Acelerometro, solo se piden 4 registros
   GyX=Wire.read()<<8|Wire.read();
   GyY=Wire.read()<<8|Wire.read();

//Calculo del angulo del Giroscopio  
   Gy[0] = GyX/G_R;
   Gy[1] = GyY/G_R;

//Filtro Complementario //一是用互補濾波器公式
   Angle[0] = 0.98 *(Angle[0]+Gy[0]*0.010) + 0.02*Acc[0];
   Angle[1] = 0.98 *(Angle[1]+Gy[1]*0.010) + 0.02*Acc[1];

//Mostrar valores
  int Uangle0 = Angle[0] * 100;
  int Uangle1 = Angle[1] * 100;

//  Serial.print("Angle X: "); Serial.print(Uangle0); Serial.print("\n");
//  Serial.print("Angle Y: "); Serial.print(Uangle1); Serial.print("\n------------\n");
  Serial.print("Angle X: "); Serial.print(Angle[0]); Serial.print("\n");
  Serial.print("Angle Y: "); Serial.print(Angle[1]); Serial.print("\n------------\n");

// establecer datos de angulos y adaptarlos a los servos  
// podr穩an variar seg繳n los servos utilizados

  if(Angle[0] > 1.5 )
    {
     servo1_angle = servo1_angle + 1;
     if ( servo1_angle > servo_max ) servo1_angle = servo_max;
    }
  else if (Angle[0] < -1.5 )
    {
     servo1_angle = servo1_angle - 1;
     if ( servo1_angle < servo_min ) servo1_angle = servo_min;
    }
  servo1.write(servo1_angle);

  if(Angle[1] > 1.5 )
   {
     servo2_angle = servo2_angle + 1;
     if ( servo2_angle > servo_max ) servo2_angle = servo_max;
    }
  else if (Angle[1] < -1.5 )
    {
     servo2_angle = servo2_angle - 1;
     if ( servo2_angle < servo_min ) servo2_angle = servo_min;
    }
servo2.write(servo2_angle);
delay(20);
  
}