#include <Wire.h>
const int MPU2=0x69,MPU1=0x68;

int16_t AcX1,AcY1,AcZ1;
int16_t AcX2,AcY2,AcZ2;
const float alpha = 1;

void setup()
{
  Wire.begin();
  Wire.beginTransmission (MPU1);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission (true);Wire.begin();
  
  Wire.beginTransmission (MPU2);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission (true);
  Serial.begin(9600);

}
void loop()
{
 Wire.beginTransmission (MPU1);
 Wire.write(0x3B);
 Wire.endTransmission (false);
 Wire.requestFrom(MPU1, 14, true);
 AcX1=Wire.read()<<8 | Wire.read();
 AcY1=Wire.read()<<8 | Wire.read();
 AcZ1=Wire.read()<<8 | Wire.read();

double fXg1 = AcX1 * alpha + (fXg1 * (1.0 - alpha));
double fYg1 = AcY1 * alpha + (fYg1 * (1.0 - alpha));
double fZg1 = AcZ1 * alpha + (fZg1 * (1.0 - alpha));

int x1 = RAD_TO_DEG * (atan2(fYg1, fZg1));
int y1 = RAD_TO_DEG * (atan2(-fXg1, -fZg1) + PI);
int z1 = RAD_TO_DEG * (atan2(-fYg1, -fXg1) + PI);

Serial.print("\n");
Serial.print(("X1 = "));
Serial.print(x1);
/* Serial.print(" | Y1 =");
Serial.print(y1);
Serial.print(" | Z1 = ");
Serial.print(z1);*/

Wire.beginTransmission(MPU2);
Wire.write(0x3b);
Wire.endTransmission(false);
Wire.requestFrom(MPU2, 14, true);
AcX2=Wire.read()<<8 | Wire.read();
AcY2=Wire.read()<<8 | Wire.read();
AcZ2=Wire.read()<<8 | Wire.read();

double fXg2 = AcX2 * alpha + (fXg2 * (1.0 - alpha));
double fYg2 = AcY2 * alpha + (fYg2 * (1.0 - alpha));
double fZg2 = AcZ2 * alpha + (fZg2 * (1.0 - alpha));

int x2 = RAD_TO_DEG * (atan2(fYg2, fZg2));
int y2 = RAD_TO_DEG * (atan2(-fXg2, -fZg2) + PI);
int z2 = RAD_TO_DEG * (atan2(-fYg2, -fXg2) + PI);

Serial.print("\t");
Serial.print("X2 = ");
Serial.print(x2);
/*Serial.print(" | Y2 = ");
Serial.print(y2);
Serial.print(" | Z2 = ");
Serial.print(z2);*/

Serial.print("\t");
Serial.print(x1-x2);
delay(500);
}