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

#define SERVO_X_PIN 3   // Servo X signal pin
#define SERVO_Y_PIN 5  // Servo Y signal pin
#define SERVO_Z_PIN 6  // Servo Z signal pin

MPU6050 mpu;
Servo servoX;  // Servo object for axis X
Servo servoY;  // Servo object for axis Y
Servo servoZ;  // Servo object for axis Z

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();

  servoX.attach(SERVO_X_PIN);
  servoY.attach(SERVO_Y_PIN);
  servoZ.attach(SERVO_Z_PIN);
}

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

  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Map the gyro readings to servo angles
  int servoAngleX = map(gx, -32768, 32767, 0, 180);
  int servoAngleY = map(gy, -32768, 32767, 0, 180);
  int servoAngleZ = map(gz, -32768, 32767, 0, 180);

  // Adjust servo positions
  servoX.write(servoAngleX);
  servoY.write(servoAngleY);
  servoZ.write(servoAngleZ);

  // Print gyro readings and servo angles for debugging
  Serial.print("Gyro X: ");
  Serial.print(gx);
  Serial.print(", Gyro Y: ");
  Serial.print(gy);
  Serial.print(", Gyro Z: ");
  Serial.print(gz);
  Serial.print(", Servo X angle: ");
  Serial.print(servoAngleX);
  Serial.print(", Servo Y angle: ");
  Serial.print(servoAngleY);
  Serial.print(", Servo Z angle: ");
  Serial.println(servoAngleZ);

  delay(100); // Adjust this delay as needed for your application
}