#include <Wire.h>
#include "MPU9250.h"

// Buat objek MPU9250
MPU9250 IMU(Wire, 0x68); // Alamat I2C MPU9250 adalah 0x68

void setup() {
  // Inisialisasi Serial Monitor
  Serial.begin(115200);

  // Inisialisasi I2C
  Wire.begin();

  // Inisialisasi MPU9250
  int status = IMU.begin();
  if (status < 0) {
    Serial.println("Gagal menginisialisasi MPU9250!");
    while (1) {} // Hentikan program jika sensor tidak terdeteksi
  }

  Serial.println("MPU9250 siap digunakan!");
}

void loop() {
  // Baca data dari MPU9250
  IMU.readSensor();

  // Ambil data accelerometer
  float ax = IMU.getAccelX_mss();
  float ay = IMU.getAccelY_mss();
  float az = IMU.getAccelZ_mss();

  // Ambil data gyroscope
  float gx = IMU.getGyroX_rads();
  float gy = IMU.getGyroY_rads();
  float gz = IMU.getGyroZ_rads();

  // Ambil data magnetometer
  float mx = IMU.getMagX_uT();
  float my = IMU.getMagY_uT();
  float mz = IMU.getMagZ_uT();

  // Tampilkan data di Serial Monitor
  Serial.print("Accelerometer (m/s^2): ");
  Serial.print(ax); Serial.print(", ");
  Serial.print(ay); Serial.print(", ");
  Serial.println(az);

  Serial.print("Gyroscope (rad/s): ");
  Serial.print(gx); Serial.print(", ");
  Serial.print(gy); Serial.print(", ");
  Serial.println(gz);

  Serial.print("Magnetometer (uT): ");
  Serial.print(mx); Serial.print(", ");
  Serial.print(my); Serial.print(", ");
  Serial.println(mz);

  // Jeda sebelum membaca data berikutnya
  delay(1000);
}