#include "Wire.h"  // Arduino Wire library
#include "I2Cdev.h"  //Installer ces 2 librairies
#include "MPU6050.h"
#include "math.h"

// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro1(0x68);
MPU6050 accelgyro2(0x69);

int16_t ax, ay, az;  //mesures brutes capteur 1
int16_t gx, gy, gz;
int16_t ax2, ay2, az2;  //mesures brutes capteur 2
int16_t gx2, gy2, gz2;
float angle1 = 0;
float angle2 = 0;


void setup() {
  Wire.begin();  //I2C bus
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB (LEONARDO)
  }

  // initialize device
  Serial.println("Initialisation I2C...");
  accelgyro1.initialize();
  accelgyro2.initialize();

  // verify connection
  Serial.println("Test de la conection du dispositif ...");
  Serial.println(accelgyro1.testConnection() ? "MPU6050 n°1 connection reussie" : "MPU6050 n°1 connection echec");
  Serial.println(accelgyro2.testConnection() ? "MPU6050 n°2 connection reussie" : "MPU6050 n°2 connection echec");

  delay(1000);
}

void loop() {

  accelgyro1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);
  angle1 = 0.98 * (angle1 + float(gy1) * 0.01 / 131) + 0.02 * atan2((double)ax1, (double)az1) * 180 / PI;

  accelgyro2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2);
  angle2 = 0.98 * (angle2 + float(gy2) * 0.01 / 131) + 0.02 * atan2((double)ax2, (double)az2) * 180 / PI;

  Serial.print(angle1);
  Serial.print("\t");
  Serial.println(angle2);
  delay(10);
}