#include <Wire.h>

// MPU6050 I2C地址 (通常是0x68，如果AD0接地)
#define MPU6050_ADDRESS 0x68

// MPU6050 寄存器地址
#define MPU6050_PWR_MGMT_1   0x6B  // 电源管理寄存器
#define MPU6050_ACCEL_XOUT_H 0x3B  // 加速度数据起始寄存器 (X轴高字节)

void setup() {
  Serial.begin(9600);  // 初始化串口通信
  Wire.begin();        // 初始化I2C

  // 唤醒 MPU6050
  Wire.beginTransmission(MPU6050_ADDRESS);
  Wire.write(MPU6050_PWR_MGMT_1);
  Wire.write(0x00);    // 写入0来唤醒传感器 (清除SLEEP位)
  Wire.endTransmission();

  Serial.println("MPU6050 初始化完成");
}

void loop() {
  float accX, accY, accZ;

  // 读取并转换加速度数据（单位：g）
  readAccel(&accX, &accY, &accZ);

  // 打印数据，保留两位小数
  Serial.print("X: ");
  Serial.print(accX, 2);
  Serial.print(" g | Y: ");
  Serial.print(accY, 2);
  Serial.print(" g | Z: ");
  Serial.print(accZ, 2);
  Serial.println(" g");

  delay(100);  // 延时100ms
}

// 读取加速度数据并转换为g单位
void readAccel(float *ax, float *ay, float *az) {
  int16_t rawX, rawY, rawZ;

  Wire.beginTransmission(MPU6050_ADDRESS);
  Wire.write(MPU6050_ACCEL_XOUT_H);  // 从X轴高字节开始读取
  Wire.endTransmission(false);       // false表示保持I2C连接，不发送STOP

  Wire.requestFrom(MPU6050_ADDRESS, 6, true); // 请求6个字节

  if (Wire.available() == 6) {
    byte xh = Wire.read();
    byte xl = Wire.read();
    byte yh = Wire.read();
    byte yl = Wire.read();
    byte zh = Wire.read();
    byte zl = Wire.read();

    rawX = (xh << 8) | xl;
    rawY = (yh << 8) | yl;
    rawZ = (zh << 8) | zl;

    // 转换为 g 单位（16384 LSB/g）
    *ax = rawX / 16384.0f;
    *ay = rawY / 16384.0f;
    *az = rawZ / 16384.0f;
  } else {
    Serial.println("Error reading MPU6050 data!");
    *ax = 0;
    *ay = 0;
    *az = 0;
  }
}