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

MPU6050 mpu;
Servo servoX;
Servo servoY;

int potXPin = A0;
int potYPin = A1;

int servoXPin = 9;
int servoYPin = 10;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  servoX.attach(servoXPin);
  servoY.attach(servoYPin);
}

void loop() {
  int16_t ax, ay, az, gx, gy, gz;

  // Read accelerometer and gyroscope data
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Map gyroscope data to servo stabilization angles
  int servoXStabilize = map(gy, -32768, 32767, -10, 10); // Adjust stabilization range
  int servoYStabilize = map(gx, -32768, 32767, -10, 10); // Adjust stabilization range

  // Read potentiometer values
  int potXValue = analogRead(potXPin);
  int potYValue = analogRead(potYPin);

  // Map potentiometer values to servo angles
  int servoXAngle = map(potXValue, 0, 1023, 0, 180);
  int servoYAngle = map(potYValue, 0, 1023, 0, 180);

  // Adjust servo angles based on stabilization
  servoXAngle += servoXStabilize;
  servoYAngle += servoYStabilize;

  // Move servo motors to the adjusted angles
  servoX.write(servoXAngle);
  servoY.write(servoYAngle);

  // Print data for debugging (optional)
  Serial.print("Gyro X: ");
  Serial.print(gx);
  Serial.print(" | Gyro Y: ");
  Serial.println(gy);

  delay(15);  // Adjust delay as needed
}