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

Servo myServo;  
MPU6050 mpu;    

int servoPin = 4;
int angle = 90;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  myServo.attach(servoPin);
}

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

  // Map gyroscope data to servo angle range
  int mappedAngle = map(gy, -2000, 2000, 0, 180);
  angle = constrain(mappedAngle, 0, 180);

  // Control servo based on gyroscope data
  myServo.write(angle);

  // Print sensor data
  Serial.print("Accel: ");
  Serial.print(ax);
  Serial.print(", ");
  Serial.print(ay);
  Serial.print(", ");
  Serial.print(az);
  Serial.print(" | Gyro: ");
  Serial.print(gx);
  Serial.print(", ");
  Serial.print(gy);
  Serial.print(", ");
  Serial.print(gz);
  Serial.print(" | Servo Angle: ");
  Serial.println(angle);

  delay(1000);
}