#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

Adafruit_MPU6050 mpu;

void setup(void) {
  Serial.begin(115200);
  while (!Serial);
  delay(100);

  Serial.println("Adafruit MPU6050 test!");

  if (!mpu.begin()) {
    Serial.println("Gagal menemukan chip MPU6050");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 ditemukan!");

  mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  Serial.println("Pengaturan sensor MPU6050 selesai\n");
  delay(100);
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  Serial.print("gyro X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);

  // Mengambil hasil data gyro X, Y, dan Z
  float hasilVektorGyro = g.gyro.x * g.gyro.x + g.gyro.y * g.gyro.y + g.gyro.z * g.gyro.z;
  hasilVektorGyro = sqrt(hasilVektorGyro);

  float gyroX = (g.gyro.x < 0) ? -1 : 1;
  float gyroY = (g.gyro.y < 0) ? -1 : 1;
  float gyroZ = (g.gyro.z < 0) ? -1 : 1;

  hasilVektorGyro = hasilVektorGyro * gyroX * gyroY * gyroZ;

  // Cetak Hasil vektor gyro
  Serial.println("");
  Serial.print("Hasil Vektor Gyro: ");
  Serial.println(hasilVektorGyro);

  delay(100);
}