#include <Servo.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

Servo servo1; 
Servo servo2;
Adafruit_MPU6050 mpu;

int pos1 = 0;
int pos2 = 0;

void setup() { 
  Wire.begin();
  Serial.begin(9600);

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.println("");
  delay(100);

  servo1.attach(11);
  servo2.attach(10);
  
  Serial.println("Init...");
  delay(3000);
  
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  /* Print out the values */
  Serial.print(a.acceleration.x);
  Serial.print(",");
  Serial.print(a.acceleration.y);
  Serial.print(",");
  Serial.print(a.acceleration.z);
  Serial.print(", ");
  Serial.print(g.gyro.x);
  Serial.print(",");
  Serial.print(g.gyro.y);
  Serial.print(",");
  Serial.print(g.gyro.z);
  Serial.println("");

  //servo1.write(map(g.gyro.x, -4.36, 4.36, 0, 180));
  //servo2.write(map(g.gyro.y, -4.36, 4.36, 0, 180));

  servo1.write(map(a.acceleration.x, -19.61, 19.61, 0, 180));
  servo2.write(map(a.acceleration.y, -19.61, 19.61, 0, 180));

  delay(10);
}