//Inclusão da biblioteca
#include <Wire.h>

//Endereços de registros de configuração
#define MPU6050 0x68 //endereço do sensor
#define CONFIG 0x1A
#define GIROSCOPIO_CONFIG 0x1B
#define ACELEROMETRO_CONFIG 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 GYROXOUTH 0x43
#define GYROXOUTL 0x44
#define GYROYOUTH 0x45
#define GYROYOUTL 0x46
#define GYROZOUTH 0x47
#define GYROZOUTL 0x48


//Declaração de variáveis globais
int contador = 1;
float giroscopio_x = 0, giroscopio_y = 0, giroscopio_z = 0;
float acelerometro_x = 0, acelerometro_y = 0, acelerometro_z = 0;

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

  Wire.beginTransmission(MPU6050); //Começa a transmissao de dados para o sensor
  Wire.write(0x6B); //enderço do sensor
  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();

}

//faixa de valores lidos: -32768 e +32767. Necessário conversão.

//Função para medição e conversão de velocidades do giroscopio
void MedirRotacao(void){
  
  Wire.beginTransmission(MPU6050);
  Wire.write(GYROXOUTH); //Pedir o registro 0x43 - corresponde ao AcX                               
  Wire.endTransmission();
  Wire.requestFrom(MPU6050, 6);  //A partir do 0x43, pedem-se 6 registros

  giroscopio_x = (Wire.read()<<8) + Wire.read(); //Cada valor ocupa 2 registros
  giroscopio_y = (Wire.read()<<8) + Wire.read();
  giroscopio_z = (Wire.read()<<8) + Wire.read();

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

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

}

//faixa de valores lidos: -32768 e +32767. Necessário conversão.
//Função para medição e conversão de acelerações do acelerômetro
void MedirAceleracao(void){
  
  Wire.beginTransmission(MPU6050);
  Wire.write(ACCELXOUTH);                                         
  Wire.endTransmission();
  Wire.requestFrom(MPU6050, 6); //A partir do 0x3B, pedem-se 6 registros

  acelerometro_x = (Wire.read()<<8) + Wire.read(); //Cada valor ocupa 2 registros
  acelerometro_y = (Wire.read()<<8) + Wire.read();
  acelerometro_z = (Wire.read()<<8) + Wire.read();

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

  if(acelerometro_y > 32767){
    acelerometro_y = (-(65536-acelerometro_y)/8192)*9.8;
  }else{
    acelerometro_y = (acelerometro_y/8192)*9.8;
  }
  
  if(acelerometro_z > 32767){
    acelerometro_z = (-(65536-acelerometro_z)/8192)*9.8;
  }else{
    acelerometro_z = (acelerometro_z/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() {

  MedirRotacao();
  MedirAceleracao();
  
  Serial.print("Medição: ");
  Serial.println(contador);
 
  Serial.print("Velocidade angular - eixo X: ");
  Serial.print(giroscopio_x);
  Serial.println(" °/s");
  Serial.print("Velocidade angular - eixo Y: ");
  Serial.print(giroscopio_y);
  Serial.println(" °/s");
  Serial.print("Velocidade angular - eixo Z: ");
  Serial.print(giroscopio_z);
  Serial.println(" °/s");

  Serial.print("Aceleração - eixo X: ");
  Serial.print(acelerometro_x);
  Serial.println(" m/s^2");
  Serial.print("Aceleração - eixo Y: ");
  Serial.print(acelerometro_y);
  Serial.println(" m/s^2");
  Serial.print("Aceleração - eixo Z: ");
  Serial.print(acelerometro_z);
  Serial.println(" m/s^2");
  
  contador++;
  Serial.println();
  delay(1000);

}