MPU-6050 Complete

#include<Wire.h>

const int MPU=0x68;
const float VALUE_PER_DEGREE = 131;
const int READ_DELAY = 25;

int16_t GyX, GyY, GyZ;
int16_t GyX_error=0, GyY_error=0, GyZ_error=0;
float HeadingX=0, HeadingY=0, HeadingZ=0;

void readGyro() {
  Wire.beginTransmission(MPU);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true);

  GyX = (Wire.read()<<8|Wire.read()) - GyX_error;
  GyY = (Wire.read()<<8|Wire.read()) - GyY_error;
  GyZ = (Wire.read()<<8|Wire.read()) - GyZ_error;
}

void updateHeading() {
  static unsigned long lastTime = 0;
  unsigned long currentTime;
  unsigned long duration;

  currentTime = millis();

  if (lastTime == 0) {
    lastTime = currentTime;
    return;
  }

  duration = currentTime - lastTime;
  lastTime = currentTime;

  HeadingX += (GyX / VALUE_PER_DEGREE) * (duration / 1000.0);
  HeadingY += (GyY / VALUE_PER_DEGREE) * (duration / 1000.0);
  HeadingZ += (GyZ / VALUE_PER_DEGREE) * (duration / 1000.0);
}

void calibrateGyro() {
  int32_t GyX_total=0, GyY_total=0, GyZ_total=0;
  int counts=80;

  for (int a=0; a<counts; a++) {
    readGyro();
    GyX_total += GyX;
    GyY_total += GyY;
    GyZ_total += GyZ;
    delay(50);
  }

  GyX_error = GyX_total / counts;
  GyY_error = GyY_total / counts;
  GyZ_error = GyZ_total / counts;
}

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

  delay(1000);
  calibrateGyro();
}

int count=0;

void loop(){
  readGyro();
  updateHeading();

  count++;
  if (count % 40 == 0) {
    Serial.print("Gyroscope: ");
    Serial.print("HX = "); Serial.print(HeadingX);
    Serial.print(" | HY = "); Serial.print(HeadingY);
    Serial.print(" | HZ = "); Serial.println(HeadingZ);
  }
  delay(READ_DELAY);
}