#include "Arduino.h"
#include "GY521.h"

GY521 sensor(0x68);

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

	// Wait for Serial because the Arduino Leonardo is weird
	while (!Serial) {}

	Wire.begin();

	delay(100);
	while (!sensor.wakeup())
	{
		Serial.print(millis());
		Serial.println("Error: Could not connect to the GY521 sensor. Retrying after 2000 milliseconds...");
		delay(2000);
	}
	sensor.setAccelSensitivity(2); // 8g
	sensor.setGyroSensitivity(1);  // 500 degrees/s

	sensor.setThrottle();
	Serial.println("start...");

	// Calibration values
	sensor.axe = 0.198;
	sensor.aye = -0.018;
	sensor.gxe = 0.780;
	sensor.gye = -1.495;
}

void loop()
{
	sensor.read();
	float pitch = sensor.getPitch();
	float roll = sensor.getRoll();

	Serial.print("Pitch: ");
	Serial.print(pitch, 3);
	Serial.print(" Roll: ");
	Serial.print(roll, 3);
	Serial.println();
}