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

MPU6050 mpu;

// Variabel untuk menyimpan nilai bias kalibrasi
int16_t axBias = 0, ayBias = 0, azBias = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 tidak terdeteksi!");
    while (1);
  }
  Serial.println("MPU6050 siap digunakan.");

  // Proses kalibrasi untuk mendapatkan nilai bias
  calibrateAccelerometer();
}

void loop() {
  int16_t ax, ay, az;
  mpu.getAcceleration(&ax, &ay, &az);

  // Mengurangi nilai dengan bias untuk hasil kalibrasi
  ax -= axBias;
  ay -= ayBias;
  az -= azBias;

  // Konversi nilai percepatan ke ADC simulasi (0-1023)
  int adcX = map(ax, -17000, 17000, 0, 1023);
  int adcY = map(ay, -17000, 17000, 0, 1023);
  int adcZ = map(az, -17000, 17000, 0, 1023);

  Serial.print("X: "); Serial.print(ax); Serial.print(" | ADC: "); Serial.print(adcX);
  Serial.print(" | Y: "); Serial.print(ay); Serial.print(" | ADC: "); Serial.print(adcY);
  Serial.print(" | Z: "); Serial.print(az); Serial.print(" | ADC: "); Serial.println(adcZ);

  delay(500);
}

// Fungsi kalibrasi untuk menentukan bias
void calibrateAccelerometer() {
  int16_t ax, ay, az;
  int32_t axSum = 0, aySum = 0, azSum = 0;
  const int sampleCount = 100;

  // Mengambil sejumlah sampel untuk kalibrasi
  for (int i = 0; i < sampleCount; i++) {
    mpu.getAcceleration(&ax, &ay, &az);
    axSum += ax;
    aySum += ay;
    azSum += az;
    delay(10);
  }

  // Menghitung rata-rata untuk mendapatkan nilai bias
  axBias = axSum / sampleCount;
  ayBias = aySum / sampleCount;
  azBias = azSum / sampleCount;
  
  // Menyesuaikan bias Z dengan percepatan gravitasi (umumnya ~16384 pada posisi diam)
  azBias -= 16384;

  Serial.println("Kalibrasi selesai:");
  Serial.print("Bias X: "); Serial.println(axBias);
  Serial.print("Bias Y: "); Serial.println(ayBias);
  Serial.print("Bias Z: "); Serial.println(azBias);
}