#include <Servo.h>
#include <Wire.h>

const int MPU = 0x68; // I2C address of the gyroscope (likely MPU-6050)
int16_t AcX, AcY, AcZ, GyroX, GyroY, GyroZ;

int centerX = 90;  // Center position for pitch servo (adjust based on your servo)
int centerY = 90;  // Center position for yaw servo (adjust based on your servo)
int threshold = 10;  // Minimum angle change for servo movement (adjust as needed)

int servoPinX = 5;  // Servo pin for pitch control
int servoPinY = 6;  // Servo pin for yaw control

Servo servoX;
Servo servoY;

float elapsedTime, currentTime, previousTime;

void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // Power on gyroscope
  Wire.write(0);     // Set wake-on-motion disabled
  Wire.endTransmission(true);
  Serial.begin(9600);
  servoX.attach(servoPinX);
  servoY.attach(servoPinY);
  previousTime = millis();
}

void loop() {
  currentTime = millis();
  elapsedTime = (currentTime - previousTime) / 1000.0;  // Time in seconds

  // Read gyroscope data
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // Register address for gyroscope data
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true);
  GyroX = Wire.read() << 8 | Wire.read();
  GyroY = Wire.read() << 8 | Wire.read();
  // ... (read other data if needed)

  // Move servos based on gyroscope readings with threshold
  int newX = centerX - GyroX / 10;  // Adjust scaling factor for pitch control
  int newY = centerY + GyroY / 10;  // Adjust scaling factor for yaw control (may need reverse)
  if (abs(newX - centerX) > threshold) {
    servoX.write(newX);
  }
  if (abs(newY - centerY) > threshold) {
    servoY.write(newY);
  }

  previousTime = currentTime;

  // Optional: Print data for debugging
  // Serial.print("Gyro: X: "); Serial.print(GyroX); Serial.print(", Y: "); Serial.println(GyroY);
  // Serial.print("Servo: X: "); Serial.print(servoX.read()); Serial.print(", Y: "); Serial.println(servoY.read());

  delay(10);  // Adjust delay based on your desired control loop speed
}