#include <Wire.h>
#define MPU6050_ADDR 0x68
void writeMPU6050(byte reg, byte data) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission();
}
byte readMPU6050(byte reg) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(reg);
  Wire.endTransmission();
  Wire.requestFrom(MPU6050_ADDR, 1);
  byte data = Wire.read();
  return data;
}
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Wire.begin();
  writeMPU6050(0X6B,0X01);
  writeMPU6050(0X1B,0X08);
  writeMPU6050(0X1C,0X00);
}

void loop() {
  // put your main code here, to run repeatedly:
  short rawAccX=0,rawAccY=0,rawAccZ=0,rawTemp=0,
        rawGyroX=0,rawGyroY=0,rawGyroZ=0;

  float temp,accX,accY,accZ,gyroX,gyroY,gyroZ;
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(MPU6050_ADDR,14);
  
  rawAccX=Wire.read()<<8||Wire.read();
  rawAccY=Wire.read()<<8||Wire.read();
  rawAccZ=Wire.read()<<8||Wire.read();
  rawTemp=Wire.read()<<8||Wire.read();
  rawGyroX=Wire.read()<<8||Wire.read();
  rawGyroY=Wire.read()<<8||Wire.read();
  rawGyroZ=Wire.read()<<8||Wire.read();
  rawGyroX=Wire.read()<<8||Wire.read();

  temp=rawTemp/340.0+36.53;
  accX=rawAccX/16384.0;
  accY=rawAccY/16384.0;
  accZ=rawAccZ/16384.0;
  gyroX=rawGyroX/65.5;
  gyroY=rawGyroY/65.5;
  gyroZ=rawGyroZ/65.5;
  Serial.printf("Temp:%.2f accX:%.2f accY:%.2f accZ:%.2f  gyroX:%.2f gyroY:%.2f gyroZ:%.2f\n ",
                      temp,accX,accY,accZ,gyroX,gyroY,gyroZ);

}