#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#define sifirla A0
// Create MPU6050 objects
Adafruit_MPU6050 mpu1;
Adafruit_MPU6050 mpu2;

// Set the LCD address to 0x27 for a 16 chars and 2 line display
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Create Servo objects
Servo servoX;
Servo servoY;
Servo servoZ;

// Define servo pins
const int servoXPin = 9;
const int servoYPin = 10;
const int servoZPin = 11;

void setup() {
  Serial.begin(115200);
  pinMode(sifirla, INPUT_PULLUP);

  // Initialize MPU6050 with address 0x68 (AD0 connected to GND)
  if (!mpu1.begin(0x68)) {
    Serial.println("Failed to find MPU6050 #1");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 #1 ready!");

  // Initialize MPU6050 with address 0x69 (AD0 connected to VCC)
  if (!mpu2.begin(0x69)) {
    Serial.println("Failed to find MPU6050 #2");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 #2 ready!");

  // Initialize the LCD
  lcd.init();
  lcd.backlight();
  lcd.print("MPU6050 ready!");
  delay(2000);
  lcd.clear();

  // Attach the servos to their respective pins
  servoX.attach(servoXPin);
  servoY.attach(servoYPin);
  servoZ.attach(servoZPin);
}

void loop() {
 
  sensors_event_t event1, event2;

  // Get accelerometer events from MPU6050 #1
  mpu1.getAccelerometerSensor()->getEvent(&event1);

  // Get accelerometer events from MPU6050 #2
  mpu2.getAccelerometerSensor()->getEvent(&event2);

  // Print the accelerometer values from MPU6050 #1
  Serial.print("MPU6050 #1 - X: ");
  Serial.print(event1.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(event1.acceleration.y);
  Serial.print(", Z: ");
  Serial.println(event1.acceleration.z);

  // Print the accelerometer values from MPU6050 #2
  Serial.print("MPU6050 #2 - X: ");
  Serial.print(event2.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(event2.acceleration.y);
  Serial.print(", Z: ");
  Serial.println(event2.acceleration.z);

  // Display the accelerometer values from MPU6050 #1 on the LCD
  lcd.setCursor(0, 0);
  lcd.print("X1: ");
  lcd.print(event1.acceleration.x, 1);
  lcd.setCursor(0, 1);
  lcd.print("X2: ");
  lcd.print(event2.acceleration.x, 1);

  // Map accelerometer values from MPU6050 #1 and #2 to servo positions (0 to 180 degrees)
  int servoXPos = map(event1.acceleration.x * 100, -1024, 1024, 0, 180);
  int servoYPos = map(event1.acceleration.y * 100, -1024, 1024, 0, 180);
  int servoZPos = map(event1.acceleration.z * 100, -1024, 1024, 0, 180);

  // Adjust the servos based on the difference between MPU6050 #1 and MPU6050 #2
  servoXPos -= map(event2.acceleration.x * 100, 1024, -1024, -90, 90);
  servoYPos -= map(event2.acceleration.y * 100, 1024, -1024,-90, 90);
  servoZPos -= map(event2.acceleration.z * 100, 1024, -1024,-90, 90);

  // Constrain the values to be within servo limits (0 to 180 degrees)
  servoXPos = constrain(servoXPos, 0, 180);
  servoYPos = constrain(servoYPos, 0, 180);
  servoZPos = constrain(servoZPos, 0, 180);

  // Write the calculated positions to the servos
  servoX.write(servoXPos);
  servoY.write(servoYPos);
  servoZ.write(servoZPos);
 if(sifirla==HIGH)
  {
 servoX.write(servoXPos);
  servoY.write(servoYPos);
  servoZ.write(servoZPos);
  }
  //delay();
}