#include <Wire.h>

// MPU6050寄存器地址
const int MPU_addr = 0x68; // I2C地址

// 传感器数据变量
int16_t AcX, AcY, AcZ, GyX, GyY, GyZ, Tmp;

// 位置和方向变量
float currentX = 0.0;
float currentY = 0.0;
float currentHeading = 0.0; // 角度 (0-360)
float temperature = 0.0;    // 温度 (°C)

// 时间变量
unsigned long lastTime = 0;
const float dt = 0.01;      // 采样时间间隔 (秒)

// 互补滤波参数
const float alpha = 0.98;   // 陀螺仪权重

void setup() {
  Serial.begin(115200);
  Wire.begin();             // 初始化I2C总线
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);         // PWR_MGMT_1寄存器
  Wire.write(0);            // 唤醒MPU6050
  Wire.endTransmission(true);
  Serial.println("MPU6050初始化成功!");
  lastTime = millis();
}

void loop() {
  // 读取MPU6050数据
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);         // 从ACCEL_XOUT_H寄存器开始读
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // 读取14个寄存器
  
  // 读取加速度、温度和陀螺仪数据
  AcX = Wire.read() << 8 | Wire.read(); // ACCEL_XOUT_H & L
  AcY = Wire.read() << 8 | Wire.read(); // ACCEL_YOUT_H & L
  AcZ = Wire.read() << 8 | Wire.read(); // ACCEL_ZOUT_H & L
  Tmp = Wire.read() << 8 | Wire.read(); // TEMP_OUT_H & L
  GyX = Wire.read() << 8 | Wire.read(); // GYRO_XOUT_H & L
  GyY = Wire.read() << 8 | Wire.read(); // GYRO_YOUT_H & L
  GyZ = Wire.read() << 8 | Wire.read(); // GYRO_ZOUT_H & L
  
  // 转换为实际单位
  float ax = AcX / 16384.0; // 加速度 (g)
  float ay = AcY / 16384.0;
  float az = AcZ / 16384.0;
  
  float gx = GyX / 131.0;   // 角速度 (°/s)
  float gy = GyY / 131.0;
  float gz = GyZ / 131.0;
  
  // 计算温度 (°C)
  temperature = Tmp / 340.0 + 36.53;
  
  // 计算时间差
  unsigned long now = millis();
  float deltaTime = (now - lastTime) / 1000.0;
  lastTime = now;
  
  // 计算倾角 (使用加速度计)
  float accelPitch = atan2(ay, az) * 180 / PI;
  float accelRoll = atan2(ax, sqrt(ay * ay + az * az)) * 180 / PI;
  
  // 积分陀螺仪数据计算角度
  static float gyroPitch = 0;
  static float gyroRoll = 0;
  gyroPitch += gx * deltaTime;
  gyroRoll += gy * deltaTime;
  
  // 互补滤波融合加速度计和陀螺仪数据
  static float pitch = 0;
  static float roll = 0;
  pitch = alpha * (pitch + gx * deltaTime) + (1 - alpha) * accelPitch;
  roll = alpha * (roll + gy * deltaTime) + (1 - alpha) * accelRoll;
  
  // 计算航向角 (使用Z轴陀螺仪)
  currentHeading += gz * deltaTime;
  
  // 限制航向角在0-360度之间
  if (currentHeading > 360) currentHeading -= 360;
  if (currentHeading < 0) currentHeading += 360;
  
  // 简单的位置积分 (仅作示例，实际应用需更复杂算法)
  // 去除重力加速度分量 (简化版)
  float gravityCompensation = 1.0; // 1g
  ax = ax; // 假设机器人在水平面上，X/Y方向不受重力影响
  
  // 位置积分 (简化版)
  static float velocityX = 0;
  static float velocityY = 0;
  
  velocityX += ax * deltaTime;
  velocityY += ay * deltaTime;
  
  currentX += velocityX * deltaTime;
  currentY += velocityY * deltaTime;
  
  // 输出数据
  printSensorData(ax, ay, gz, temperature, currentX, currentY, currentHeading);
  
  delay(10); // 短暂延迟
}

void printSensorData(float ax, float ay, float gz, float temp, float x, float y, float heading) {
  Serial.print("位置: (");
  Serial.print(x, 2);
  Serial.print(", ");
  Serial.print(y, 2);
  Serial.print(") | 方向: ");
  Serial.print(heading, 1);
  Serial.print("° | 温度: ");
  Serial.print(temp, 1);
  Serial.print("°C | 加速度: (");
  Serial.print(ax, 2);
  Serial.print(", ");
  Serial.print(ay, 2);
  Serial.print(") | 旋转: ");
  Serial.print(gz, 2);
  Serial.println();
}