//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 GyroX, GyroY, GyroZ;

float GyroErrorX, GyroErrorY, GyroErrorZ;

int c;

void setup() {
  Serial.begin(9600);
  
  // 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:

}