#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "Flight_Functions.h"

void setup() {
  Serial.begin(115200);
  // Initialize MPU6050
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B); // Power management register
  Wire.write(0);    // Wake up MPU6050
  Wire.endTransmission(true);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  mpu.begin();
}
void loop() {
  Gyro_PrintInfo();
}