#include <Wire.h>

//Endereços de registros de configuração
#define MPU6050 0x68
#define CONFIG 0x1A
#define GYROCONFIG 0x1B
#define ACCELCONFIG 0x1C
#define I2CMSLCTRL 0x24
#define PWRMGMT1 0x6B

//Endereços de registros de dados medidos
#define ACCELXOUTH 0x3B
#define ACCELXOUTL 0x3C
#define ACCELYOUTH 0x3D
#define ACCELYOUTL 0x3E
#define ACCELZOUTH 0x3F
#define ACCELZOUTL 0x40
#define TEMPOUTH 0x41
#define TEMPOUTL 0x42
#define GYROXOUTH 0x43
#define GYROXOUTL 0x44
#define GYROYOUTH 0x45
#define GYROYOUTL 0x46
#define GYROZOUTH 0x47
#define GYROZOUTL 0x48

//Declaração de variáveis globais
int count = 1;
float temperatura = 0;
float gyrox = 0, gyroy = 0, gyroz = 0;
float accelx = 0, accely = 0, accelz = 0;

//Configuração inicial do MPU6050
void Config_MPU6050(void){

  Wire.beginTransmission(MPU6050);
  Wire.write(0x6B);
  Wire.write(0x00); //Configurações padrão de funcionamento
  Wire.endTransmission();
  Wire.beginTransmission(MPU6050);
  Wire.write(CONFIG);
  Wire.write(0x05); //Filtro passa-baixa em 10Hz
  Wire.write(0x08); //Range de medição: +- 500°/s
  Wire.write(0x08); //+- 4g
  Wire.endTransmission();
  Wire.beginTransmission(MPU6050);
  Wire.write(I2CMSLCTRL);
  Wire.write(0x08); //Velocidade de transmissão: 400 kHz
  Wire.endTransmission();

}

//Função para medição e conversão da temperatura
void MedeTemp(void){

  Wire.beginTransmission(MPU6050);
  Wire.write(TEMPOUTH);                                            
  Wire.endTransmission();
  Wire.requestFrom(MPU6050, 2);    
  
  temperatura = (Wire.read()<<8) + Wire.read();

    if(temperatura>20000){
      temperatura = 36.53 - (65535 - temperatura)/340;
    }else{
      temperatura = temperatura/340 + 36.53;
    }

}

//Função para medição e conversão de velocidades do giroscopio
void MedeGyro(void){
  
  Wire.beginTransmission(MPU6050);
  Wire.write(GYROXOUTH);                                         
  Wire.endTransmission();
  Wire.requestFrom(MPU6050, 6); 

  gyrox = (Wire.read()<<8) + Wire.read();
  gyroy = (Wire.read()<<8) + Wire.read();
  gyroz = (Wire.read()<<8) + Wire.read();

  if(gyrox > 32767){
    gyrox = -(65536-gyrox)/65.5;
  }else{
    gyrox = gyrox/65.5;
  }

  if(gyroy > 32767){
    gyroy = -(65536-gyroy)/65.5;
  }else{
    gyroy = gyroy/65.5;
  }
  
  if(gyroz > 32767){
    gyroz = -(65536-gyroz)/65.5;
  }else{
    gyroz = gyroz/65.5;
  }

}

//Função para medição e conversão de acelerações do acelerômetro
void MedeAccel(void){
  
  Wire.beginTransmission(MPU6050);
  Wire.write(ACCELXOUTH);                                         
  Wire.endTransmission();
  Wire.requestFrom(MPU6050, 6); 

  accelx = (Wire.read()<<8) + Wire.read(); 
  accely = (Wire.read()<<8) + Wire.read();
  accelz = (Wire.read()<<8) + Wire.read();

  if(accelx > 32767){
    accelx = (-(65536-accelx)/8192)*9.8;
  }else{
    accelx = (accelx/8192)*9.8;
  }

  if(accely > 32767){
    accely = (-(65536-accely)/8192)*9.8;
  }else{
    accely = (accely/8192)*9.8;
  }
  
  if(accelz > 32767){
    accelz = (-(65536-accelz)/8192)*9.8;
  }else{
    accelz = (accelz/8192)*9.8;
  }

}

void setup(void) {

  Serial.begin(9600);
  Wire.begin();
  Config_MPU6050();
  Serial.println();
  Serial.println("Configuracoes MPU6050:");
  Serial.println();
  Serial.println("FPB: 10Hz");
  Serial.println("Range do acelerometro: +-4g");
  Serial.println("Range do giroscopio: +- 500°/s");
  Serial.println("Velocidade de transmissão: 400 kHz");
  Serial.println();

}

void loop() {

  MedeTemp();
  MedeGyro();
  MedeAccel();
  
  Serial.print("Medição: ");
  Serial.println(count);

  Serial.print("Temperatura: ");
  Serial.print(temperatura);
  Serial.println(" °C");
  
  Serial.print("Velocidade angular no eixo X: ");
  Serial.print(gyrox);
  Serial.println(" °/s");
  Serial.print("Velocidade angular no eixo Y: ");
  Serial.print(gyroy);
  Serial.println(" °/s");
  Serial.print("Velocidade angular no eixo Z: ");
  Serial.print(gyroz);
  Serial.println(" °/s");

  Serial.print("Aceleração no eixo X: ");
  Serial.print(accelx);
  Serial.println(" m/s^2");
  Serial.print("Aceleração no eixo Y: ");
  Serial.print(accely);
  Serial.println(" m/s^2");
  Serial.print("Aceleração no eixo Z: ");
  Serial.print(accelz);
  Serial.println(" m/s^2");
  
  count++;
  Serial.println();
  delay(1000);

}