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

// Define IMU object
MPU6050 mpu(Wire);

// Define Servo objects
Servo servo1, servo2;

void setup() {
  // Initialize IMU
  Wire.begin();
  mpu.begin();
  mpu.calcGyroOffsets(); // Calibrate gyroscope offsets

  // Initialize servos
  servo1.attach(9); // Pin 9 for servo 1
  servo2.attach(10); // Pin 10 for servo 2

  // Set servos to 90 degrees
  servo1.write(90);
  servo2.write(90);
}

void loop() {
  // Update sensor data
  mpu.update();

  // Get angles from accelerometer and gyroscope
  float angleX = mpu.getAngleX();
  float angleY = mpu.getAngleY();

  // Adjust servo positions based on angles
  // Assuming angles are in degrees
  int servo1Pos = 90 + angleX;
  int servo2Pos = 90 - angleY; // Adjust direction if necessary

  // Constrain servo positions to valid range (0 to 180 degrees)
  servo1Pos = constrain(servo1Pos, 0, 180);
  servo2Pos = constrain(servo2Pos, 0, 180);

  // Update servo positions
  servo1.write(servo1Pos);
  servo2.write(servo2Pos);
}