#include <Wire.h>

const int MPU = 0x68; 
float AccX_Raw, AccY_Raw, AccZ_Raw;
float AccX, AccY, AccZ;
float AccX_SI, AccY_SI, AccZ_SI;
float roll_rad, pitch_rad;
float roll_deg, pitch_deg;

void setup() {
Serial.begin (9600);    
Wire.begin ();         
Wire.beginTransmission (MPU);     
Wire.write (0x6B);       
Wire.write (0x00);       
Wire.endTransmission (true);   

//Setting sensitivitas accelerometer
Wire.beginTransmission (MPU);
Wire.write (0x1C);      
Wire.write (0x10);      
                        
Wire.endTransmission (true);   

delay (20);
}

void loop() {
  // Pembacaan data accelerometer //
Wire.beginTransmission (MPU);   
Wire.write (0x3B);               
Wire.endTransmission (false);   
Wire.requestFrom (MPU,6,true);  
AccX_Raw = Wire.read()<<8|Wire.read();  
AccY_Raw = Wire.read()<<8|Wire.read();  
AccZ_Raw = Wire.read()<<8|Wire.read();  

//sensitivitas akleserometer di atur pada nilai +/- 8g, sehingga nilai pembacaan data register perlu dibagi 4096 untuk mengkonversi Raw data ke satuan g-force (lihat datasheet)
AccX = AccX_Raw/4096;     
AccY = AccY_Raw/4096;
AccZ = AccZ_Raw/4096;

roll_rad = atan2((AccY) , sqrt(AccX * AccX + AccZ * AccZ));    //persamaan untuk mengkonversi G-force menjadi sudut Roll (satuan radian)
//roll_rad = atan2(AccY, AccZ); //hasilnya sama               
pitch_rad = atan2((-AccX) , sqrt(AccY * AccY + AccZ * AccZ));   //persamaan untuk mengkonversi G-force menjadi sudut Pitch (satuan radian)

roll_deg = roll_rad * 180/3.14;     //satuan derajat
pitch_deg = pitch_rad * 180/3.14;   //satuan derajat

//tampilkan hasil pengukuran di serial monitor
Serial.print ("Roll = ");
Serial.println (roll_deg);
Serial.print ("Pitch = ");
Serial.println (pitch_deg);
delay (500);
}