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

const int SERVO_X_PIN = 9;
const int SERVO_Y_PIN = 10;
const int SERVO_Z_PIN = 11;

Servo servo_x;          
Servo servo_y;          
Servo servo_z;          
MPU6050 mpu;

void setup() { 
  Wire.begin();
  Serial.begin(9600); 
  
  servo_x.attach(SERVO_X_PIN);
  servo_y.attach(SERVO_Y_PIN);
  servo_z.attach(SERVO_Z_PIN);

  Serial.println("Initializing the mpu"); 
  mpu.initialize(); 
  Serial.println (mpu.testConnection() ? "Successfully Connected" : "Connection failed"); 
  delay(500); 
  Serial.println("Taking Values from the mpu");
  delay(500);
}

void loop() { 
  int16_t gx, gy, gz;
  mpu.getRotation(&gx, &gy, &gz);

  gx = map(gx, -17000, 17000, 0, 180);
  gy = map(gy, -17000, 17000, 0, 180);
  gz = map(gz, -17000, 17000, 0, 180); 

  Serial.print("x: ");
  Serial.print(gx);
  Serial.print("   y: ");
  Serial.print(gy);
  Serial.print("   z: ");
  Serial.println(gz);
  
  servo_x.write(gx); 
  servo_y.write(gy); 
  servo_z.write(gz); 

  delay(200);
}