#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  
  // Khởi tạo MPU6050
  Serial.println("Đang khởi động MPU6050...");
  if (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) {
    Serial.println("Không tìm thấy MPU6050. Vui lòng kiểm tra kết nối.");
    while (1);
  }
  Serial.println("MPU6050 đã khởi động thành công!");

  // Cấu hình cảm biến
  mpu.setThreshold(3); // Ngưỡng để lọc bớt nhiễu
}

void loop() {
  // Đọc giá trị gia tốc
  Vector normAccel = mpu.readNormalizeAccel();

  // Hiển thị tọa độ
  Serial.print("Tọa độ X: ");
  Serial.print(normAccel.XAxis);
  Serial.print(" m/s^2\t");
  
  Serial.print("Tọa độ Y: ");
  Serial.print(normAccel.YAxis);
  Serial.print(" m/s^2\t");
  
  Serial.print("Tọa độ Z: ");
  Serial.print(normAccel.ZAxis);
  Serial.println(" m/s^2");

  delay(500); // Đợi 500ms trước khi đọc lần tiếp theo
}