Accelerometer and Gyroscope I2C MPU-6050

#include <Wire.h>

const uint8_t MPU_ADDR = 0x68; // AD0 low on the module
const uint8_t PWR_MGMT_1 = 0x6B;
const uint8_t ACCEL_XOUT_H = 0x3B;

void writeRegister(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission(true);
}

void readBytes(uint8_t reg, uint8_t count, uint8_t *buffer) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, count, true);
  for (uint8_t i = 0; i < count && Wire.available(); i++) {
    buffer[i] = Wire.read();
  }
}

int16_t toInt16(uint8_t highByte, uint8_t lowByte) {
  return (int16_t)((highByte << 8) | lowByte);
}

void setup() {
  Serial.begin(9600);
  while (!Serial) {
    ;
  }

  Wire.begin();
  Wire.setClock(400000);

  // Wake up the MPU-6050 because it starts in sleep mode.
  writeRegister(PWR_MGMT_1, 0x00);
  delay(100);

  Serial.println("MPU-6050 accelerometer + gyroscope demo");
  Serial.println("Reading raw acceleration and gyroscope data...");
}

void loop() {
  uint8_t raw[14];
  readBytes(ACCEL_XOUT_H, 14, raw);

  int16_t accelX = toInt16(raw[0], raw[1]);
  int16_t accelY = toInt16(raw[2], raw[3]);
  int16_t accelZ = toInt16(raw[4], raw[5]);
  int16_t tempRaw = toInt16(raw[6], raw[7]);
  int16_t gyroX = toInt16(raw[8], raw[9]);
  int16_t gyroY = toInt16(raw[10], raw[11]);
  int16_t gyroZ = toInt16(raw[12], raw[13]);

  float tempC = (tempRaw / 340.0) + 36.53;

  Serial.print("Accel [LSB] X:");
  Serial.print(accelX);
  Serial.print(" Y:");
  Serial.print(accelY);
  Serial.print(" Z:");
  Serial.print(accelZ);

  Serial.print("  Gyro [LSB] X:");
  Serial.print(gyroX);
  Serial.print(" Y:");
  Serial.print(gyroY);
  Serial.print(" Z:");
  Serial.print(gyroZ);

  Serial.print("  Temp:");
  Serial.print(tempC, 2);
  Serial.println(" C");

  delay(500);
}