//Menghitung Error pembacaan akselerasi dan gyroskop, ini bisa dijadikan fungsi yang dibaca pada void.setup(), hasil pembacaan error pada program ini bisa diaplikasikan ke sketch pembacaan akselerasi 
//dan gyroskop agar membuat hasil pembacaan sensor menjadi lebih akurat. 
//saat pembacaan error letakan sensor pada permukaan datar dan jangan dulu digerakan sampai pembacaan error selesai.

#include <Wire.h>

const int MPU = 0x68;
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float roll_deg, pitch_deg;

float AccErrorX, AccErrorY;
float GyroErrorX, GyroErrorY, GyroErrorZ;

int c;

void setup() {
  Serial.begin(9600);

  // baca nilai akselerasi 200 kali
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;

    // Sum all readings
    roll_deg = (atan2((AccY) , sqrt(AccX * AccX + AccZ * AccZ))) * 180/3.14 ;
    pitch_deg = (atan2((-AccX) , sqrt(AccY * AccY + AccZ * AccZ))) * 180/3.14;

    AccErrorX = AccErrorX + AccX_deg;
    AccErrorY = AccErrorY + AccY_deg;
    c++;

    //untuk melihat nilai AccError setiap loop 
    Serial.print ("AccError loop ke - ");
    Serial.print (c);
    Serial.print (" = ");
    Serial.print (AccErrorX);
    Serial.print ("||");
    Serial.println (AccErrorY);
  }
  //Divide the sum by 200 to get the error value
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  c = 0;

  // Read gyro values 200 times

  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    GyroX = Wire.read() << 8 | Wire.read();
    GyroY = Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();
    // Sum all readings
    GyroErrorX = GyroErrorX + (GyroX / 131.0);
    GyroErrorY = GyroErrorY + (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
    c++;

    //untuk melihat nilai GyroError setiap loop 
    Serial.print ("Gyro Error loop ke - ");
    Serial.print (c);
    Serial.print (" = ");
    Serial.print (GyroErrorX);
    Serial.print ("||");
    Serial.print (GyroErrorY);
    Serial.print ("||");
    Serial.println (GyroErrorZ);
  }

  //Divide the sum by 200 to get the error value
  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
  GyroErrorZ = GyroErrorZ / 200;


  // Print the error values on the Serial Monitor
  Serial.print("AccErrorX: ");
  Serial.println(AccErrorX);
  Serial.print("AccErrorY: ");
  Serial.println(AccErrorY);
  Serial.print("GyroErrorX: ");
  Serial.println(GyroErrorX);
  Serial.print("GyroErrorY: ");
  Serial.println(GyroErrorY);
  Serial.print("GyroErrorZ: ");
  Serial.println(GyroErrorZ);
}

void loop() {
  // put your main code here, to run repeatedly:

}