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

Adafruit_MPU6050 mpu;
Servo sg90;
int servo_pin = 6;
void setup(void) {
	sg90.attach(servo_pin);
	Serial.begin(115200);
	// Try to initialize!
	if (!mpu.begin()) {
		Serial.println("Failed to find MPU6050 chip");
		while (1) {
		  delay(10);
		}
	}
	Serial.println("MPU6050 Found!");

	// set accelerometer range to +-8G
	mpu.setAccelerometerRange(MPU6050_RANGE_8_G);

	// set gyro range to +- 500 deg/s
	mpu.setGyroRange(MPU6050_RANGE_500_DEG);

	// set filter bandwidth to 21 Hz
	mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

	delay(100);
}

void loop() {
	/* Get new sensor events with the readings */
	sensors_event_t a, g, temp;
	mpu.getEvent(&a, &g, &temp);

	/* Print out the values */
	//Serial.print("Acceleration X: ");
	//Serial.print(a.acceleration.x);
	//Serial.print(", Y: ");
	//Serial.print(a.acceleration.y);
	//Serial.print(", Z: ");
	//Serial.print(a.acceleration.z);
	//Serial.println(" m/s^2");
	
	Serial.print(millis());
	Serial.print("Rotation X: ");
	Serial.print(g.gyro.x*57.2957795,5);
	Serial.print(", Y: ");
	Serial.print(g.gyro.y*57.2957795,5);
	Serial.print(", Z: ");
	Serial.print(g.gyro.z*57.2957795,5);
	Serial.println(" °/s");

	sg90.write(g.gyro.x*57.2957795);

	//Serial.print("Temperature: ");
	//Serial.print(temp.temperature);
	//Serial.println(" degC");
	delay(3);
}