#include <Wire.h>
float rateRoll, ratePitch, rateYaw;
float accX, accY, accZ;
float angleRoll, anglePitch;

void setupIMU() {
    // Inisialisasi MPU6050
    Wire.beginTransmission(0x68);
    Wire.write(0x6B);
    Wire.write(0x00); // Membebaskan dari mode tidur
    Wire.endTransmission();

    // Pengaturan filter akselerometer
    Wire.beginTransmission(0x68);
    Wire.write(0x1C); 
    Wire.write(0x10); // Pengaturan sensivitas +/- 8g
    Wire.endTransmission();

    // Pengaturan filter giroskop
    Wire.beginTransmission(0x68);
    Wire.write(0x1B); 
    Wire.write(0x08); // Pengaturan sensitivitas +/- 500 deg/s
    Wire.endTransmission();
}

void gyroSignal() {
    // Baca data akselerometer
    Wire.beginTransmission(0x68);
    Wire.write(0x3B);
    Wire.endTransmission();
    Wire.requestFrom(0x68, 6);
    int16_t accXLSB = Wire.read() << 8 | Wire.read();
    int16_t accYLSB = Wire.read() << 8 | Wire.read();
    int16_t accZLSB = Wire.read() << 8 | Wire.read();

    // Baca data giroskop
    Wire.beginTransmission(0x68);
    Wire.write(0x43);
    Wire.endTransmission();
    Wire.requestFrom(0x68, 6);
    int16_t gyroX = Wire.read() << 8 | Wire.read();
    int16_t gyroY = Wire.read() << 8 | Wire.read();
    int16_t gyroZ = Wire.read() << 8 | Wire.read();

    // Hitung kecepatan sudut giroskop
    rateRoll = (float)gyroX / 65.5;
    ratePitch = (float)gyroY / 65.5;
    rateYaw = (float)gyroZ / 65.5;

    // Hitung akselerasi
    accX = (float)accXLSB / 4096.0;
    accY = (float)accYLSB / 4096.0;
    accZ = (float)accZLSB / 4096.0;

    // Hitung sudut menggunakan akselerometer
    angleRoll = atan2(accY, sqrt(accX * accX + accZ * accZ)) * (180.0 / PI);
    anglePitch = -atan2(accX, sqrt(accY * accY + accZ * accZ)) * (180.0 / PI);
}

void setup() {
    Serial.begin(57600);
    Wire.setClock(400000);
    Wire.begin();
    setupIMU();
}

void loop() {
    gyroSignal();
    Serial.print("Acc X: ");
    Serial.print(accX, 2);
    Serial.print(" Acc Y: ");
    Serial.print(accY, 2);
    Serial.print(" Acc Z: ");
    Serial.print(accZ, 2);
    Serial.print(" Roll: ");
    Serial.print(angleRoll, 2);
    Serial.print(" Pitch: ");
    Serial.println(anglePitch, 2);
    delay(100);
}