MPU-6050 Intermediate

#include<Wire.h>

const int MPU=0x68;

int16_t GyX, GyY, GyZ;
int16_t GyX_error=0, GyY_error=0, GyZ_error=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 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();
}

void loop(){
  readGyro();

  Serial.print("Gyroscope: ");
  Serial.print("X = ");
  Serial.print(GyX);
  Serial.print(" | Y = ");
  Serial.print(GyY);
  Serial.print(" | Z = ");
  Serial.println(GyZ);

  delay(500);
}