#include <Wire.h>//Biblioteca para comunicação I2C

const int MPU1 = 0x68; // Endereço do sensor 1
const int MPU2 = 0x69; // Endereço do sensor 2

int16_t AcX1,AcY1,AcZ1,Tmp1,GyX1,GyY1,GyZ1; //Variaveis para pegar os valores medidos
int16_t AcX2,AcY2,AcZ2,Tmp2,GyX2,GyY2,GyZ2; //Variaveis para pegar os valores medidos

void setup(){
  Wire.begin(); // Inicia a comunicação I2C
  Wire.beginTransmission(MPU1); //Começa a transmissao de dados para o sensor 1
  Wire.write(0x6B); // registrador PWR_MGMT_1
  Wire.write(0); // Manda 0 e "acorda" o sensor 1
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU2); //Começa a transmissao de dados para o sensor 2
  Wire.write(0x6B); // registrador PWR_MGMT_1
  Wire.write(0); // Manda 0 e "acorda" o sensor 2
  Wire.endTransmission(true);

  Serial.begin(9600); //Inicia a comunicaçao serial (para exibir os valores lidos)
}
void loop(){
  // Sensor 1
  Wire.beginTransmission(MPU1); //Começa a transmissao de dados do sensor 1
  Wire.write(0x3B); // Registrador dos dados medidos (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU1,14,true); // Faz um "pedido" para ler 14 registradores, que serão os registrados com os dados medidos
  AcX1 = Wire.read()<<8|Wire.read();
  AcY1 = Wire.read()<<8|Wire.read();
  AcZ1 = Wire.read()<<8|Wire.read();
  Tmp1 = Wire.read()<<8|Wire.read();
  GyX1 = Wire.read()<<8|Wire.read();
  GyY1 = Wire.read()<<8|Wire.read();
  GyZ1 = Wire.read()<<8|Wire.read();
  Wire.endTransmission(true); // Se der erro tente tirar esta linha

  // Sensor 2
  Wire.beginTransmission(MPU2); // Começa a transmissao de dados do sensor 2
  Wire.write(0x3B); // Registrador dos dados medidos (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU2,14,true); // Faz um "pedido" para ler 14 registradores, que serão os registrados com os dados medidos
  AcX2 = Wire.read()<<8|Wire.read();
  AcY2 = Wire.read()<<8|Wire.read();
  AcZ2 = Wire.read()<<8|Wire.read();
  Tmp2 = Wire.read()<<8|Wire.read();
  GyX2 = Wire.read()<<8|Wire.read();
  GyY2 = Wire.read()<<8|Wire.read();
  GyZ2 = Wire.read()<<8|Wire.read();
  Wire.endTransmission(true); // Se der erro tente tirar esta linha

  // Agora escreve os valores no monitor serial
  // Sensor 1
  Serial.print("AcX1 = "); Serial.print(AcX1);
  Serial.print(" | AcY1 = "); Serial.print(AcY1);
  Serial.print(" | AcZ1 = "); Serial.print(AcZ1);
  Serial.print(" | Tmp1 = "); Serial.print(Tmp1/340.00+36.53); // Equação da temperatura em Cº de acordo com o datasheet
  Serial.print(" | GyX1 = "); Serial.print(GyX1);
  Serial.print(" | GyY1 = "); Serial.print(GyY1);
  Serial.print(" | GyZ1 = "); Serial.println(GyZ1);
  // Sensor 2
  Serial.print("AcX2 = "); Serial.print(AcX2);
  Serial.print(" | AcY2 = "); Serial.print(AcY2);
  Serial.print(" | AcZ2 = "); Serial.print(AcZ2);
  Serial.print(" | Tmp2 = "); Serial.print(Tmp2/340.00+36.53); // Equação da temperatura em Cº de acordo com o datasheet
  Serial.print(" | GyX2 = "); Serial.print(GyX2);
  Serial.print(" | GyY2 = "); Serial.print(GyY2);
  Serial.print(" | GyZ2 = "); Serial.println(GyZ2);

  delay(500);
}