#include <Wire.h>          // Thư viện cho giao tiếp I2C
#include <MPU6050.h>      // Thư viện cho cảm biến MPU6050

MPU6050 mpu;              // Tạo đối tượng MPU6050

void setup() {
  Serial.begin(115200);   // Khởi động giao tiếp Serial với tốc độ 115200 bps
  Wire.begin();           // Khởi động giao tiếp I2C
  mpu.initialize();       // Khởi tạo MPU6050
  
  // Kiểm tra kết nối với MPU6050
  if (!mpu.testConnection()) {
    Serial.println("Kết nối MPU6050 thất bại"); // Nếu không kết nối được
    while (1);           // Dừng chương trình
  }
}

void loop() {
  int16_t ax, ay, az;    // Khai báo biến để lưu trữ giá trị gia tốc

  // Đọc dữ liệu gia tốc từ cảm biến
  mpu.getAcceleration(&ax, &ay, &az);
  
  // Tính toán góc nghiêng theo trục X và Y
  float angleX = atan2(ay, az) * 180 / PI; // Tính góc X
  float angleY = atan2(ax, az) * 180 / PI; // Tính góc Y

  // In ra góc nghiêng
  Serial.print("Góc X: ");
  Serial.print(angleX);  // In góc X
  Serial.print(" | Góc Y: ");
  Serial.println(angleY); // In góc Y

  // Kiểm tra trạng thái cân bằng
  if (abs(angleX) < 5 && abs(angleY) < 5) {
    Serial.println("Cân bằng"); // Nếu góc X và Y nhỏ hơn 5 độ
  } else {
    if (angleX > 5) {
      Serial.println("Nghiêng phải"); // Nghiêng phải
    } else if (angleX < -5) {
      Serial.println("Nghiêng trái");  // Nghiêng trái
    }
    if (angleY > 5) {
      Serial.println("Nghiêng xuống"); // Nghiêng xuống
    } else if (angleY < -5) {
      Serial.println("Nghiêng lên");    // Nghiêng lên
    }
  }

  delay(500); // Thời gian chờ giữa các lần đọc
}