#include <Wire.h>
#include <MPU6050.h>

// Define the I2C addresses of the MPU6050 sensors
const uint8_t MPU1_ADDRESS = 0x68;
const uint8_t MPU2_ADDRESS = 0x69;

MPU6050 mpu1(MPU1_ADDRESS);
MPU6050 mpu2(MPU2_ADDRESS);
double x_1, y_1, z_1;
double x2, y2, z2;
void setup() {
  Wire.begin();

  // Initialize MPU6050 sensors with their respective addresses
  mpu1.initialize();
  mpu2.initialize();

  // Set MPU6050 scale and sensitivity settings if needed
  // mpu1.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  //  mpu1.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  // mpu2.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  // mpu2.setFullScaleGyroRange(MPU6050_GYRO_FS_250);

  // Start serial communication
  Serial.begin(115200);
}

void loop() {
  // Read accelerometer and gyroscope data from MPU1
  int16_t ax1, ay1, az1, gx1, gy1, gz1;
  mpu1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);

  // Read accelerometer and gyroscope data from MPU2
  int16_t ax2, ay2, az2, gx2, gy2, gz2;
  mpu2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2);

  // Print the data
  Serial.print("MPU1: ");
  //  Serial.print("Accel(XYZ): ");
  //  Serial.print(ax1); Serial.print(", ");
  //  Serial.print(ay1); Serial.print(", ");
  //  Serial.print(az1); Serial.print(" | ");
  //Serial.print("Gyro(XYZ): ");
  x_1 = gx1 + 171;
  y_1 = gy1 - 2379;
  z_1 = gz1 + 132;
  //  Serial.print(gx1); Serial.print(", ");
  //  Serial.print(gy1); Serial.print(", ");
  //  Serial.println(gz1);
  if (x_1 > -40 && x_1 < 40) {
    x_1 = 0;
  }
  if (y_1 > -40 && y_1 < 40) {
    y_1 = 0;
  }
  if (z_1 > -40 && z_1 < 40) {
    z_1 = 0;
  }
  Serial.print(x_1); Serial.print(", ");
  Serial.print(y_1); Serial.print(", ");
  Serial.println(z_1);

  Serial.print("MPU2: ");
  //  Serial.print("Accel(XYZ): ");
  //  Serial.print(ax2); Serial.print(", ");
  //  Serial.print(ay2); Serial.print(", ");
  //  Serial.print(az2); Serial.print(" | ");
  x2 = gx2 + 390;
  y2 = gy2 + 80;
  z2 = gz2 + 60;
  //  Serial.print(gx2); Serial.print(", ");
  //  Serial.print(gy2); Serial.print(", ");
  //  Serial.println(gz2);
  if (x2 > -40 && x2 < 40) {
    x2 = 0;
  }
  if (y2 > -40 && y2 < 40) {
    y2 = 0;
  }
  if (z2 > -40 && z2 < 40) {
    z2 = 0;
  }
  Serial.print(x2); Serial.print(", ");
  Serial.print(y2); Serial.print(", ");
  Serial.println(z2);
  Serial.println("------------------------------------------------------");
  delay(500); // Adjust the delay according to your requirements
}