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

// Define servo pins
#define PITCH_PIN 9
#define YAW_PIN 10
#define ROLL_PIN 11

// Define MPU6050 pins
#define SDA_PIN A4
#define SCL_PIN A5

// Define servo objects
Servo pitchServo;
Servo yawServo;
Servo rollServo;

// Define MPU6050 object
MPU6050 mpu;

// Variables for storing servo positions
int pitchPos = 0;
int yawPos = 0;
int rollPos = 0;

// Variables for storing MPU6050 data
int16_t accelX, accelY, accelZ;
int16_t gyroX, gyroY, gyroZ;

// Function to map MPU6050 data to servo positions
int mapServo(int value) {
  return map(value, -32768, 32767, 0, 180);
}

void setup() {
  // Initialize serial communication
  Serial.begin(9600);

  // Initialize MPU6050
  mpu.initialize();

  // Attach servo objects to pins
  pitchServo.attach(PITCH_PIN);
  yawServo.attach(YAW_PIN);
  rollServo.attach(ROLL_PIN);

  // Move servos to initial position (0 degrees)
  pitchServo.write(0);
  yawServo.write(0);
  rollServo.write(0);
}

void loop() {
  // Read MPU6050 data
  mpu.getMotion6(&accelX, &accelY, &accelZ, &gyroX, &gyroY, &gyroZ);

  // Map MPU6050 data to servo positions
  pitchPos = mapServo(-gyroX);
  yawPos = mapServo(-gyroY);
  rollPos = mapServo(-gyroZ);

  // Move servos to correct position
  pitchServo.write(pitchPos);
  yawServo.write(yawPos);
  rollServo.write(rollPos);

  // Print servo positions to serial monitor
  Serial.print("Pitch: ");
  Serial.print(pitchPos);
  Serial.print(" | Yaw: ");
  Serial.print(yawPos);
  Serial.print(" | Roll: ");
  Serial.println(rollPos);

  // Delay for stability
  delay(100);
}

// Code sourced from Arduino MPU6050 library and Servo library examples.