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

MPU6050 sensor;
int16_t ax, ay, az;
int16_t gx, gy, gz;

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

void setup() {
  Wire.begin();
  sensor.initialize();
  servo1.attach(6);
  servo2.attach(9);
  servo3.attach(10);
  servo4.attach(11);
}

void loop() {
  sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  int servo1Pos = map(ax, -17000, 17000, 0, 180);
  int servo2Pos = map(ay, -17000, 17000, 0, 180);
  int servo3Pos = map(gx, -20000, 20000, 0, 180);
  int servo4Pos = map(gy, -20000, 20000, 0, 180);

  servo1.write(servo1Pos);
  servo2.write(servo2Pos);
  servo3.write(servo3Pos);
  servo4.write(servo4Pos);

  delay(15);
}