// MPU6050 example
// https://wokwi.com/arduino/projects/305937248748044864

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
Adafruit_MPU6050 mpu;

LiquidCrystal_I2C lcd(0x27, 20, 4); 
#include<Servo.h>
Servo Myservo;
float x,y,z;
void setup(void) {
  lcd.begin(20, 4);
  lcd.init();                            // Inicializa o display
  lcd.backlight();                       // Acende a luz de fundo do display
  lcd.clear();                           // Limpa o display
   Myservo.attach(3);
  Serial.begin(115200);

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  
}

sensors_event_t event;

void loop() {
  mpu.getAccelerometerSensor()->getEvent(&event);
 x=map(event.acceleration.x,-19.61,19.61,-360,360);
 y=map(event.acceleration.y,-19.61,19.61,-360,360);
 z=map(event.acceleration.z,-19.61,19.61,-360,360);

 lcd.clear();
// Envia valor X do acelerômetro para o LCD
    lcd.setCursor(0, 0);
    lcd.print("X=");
    lcd.setCursor(3, 0);
    lcd.print(x);
      // Envia valor Y do acelerômetro para o LCD
    lcd.setCursor(0, 1);
    lcd.print("Y=");
    lcd.setCursor(3, 1);
    lcd.print(y);
// Envia valor Z do acelerômetro para o LCD
    lcd.setCursor(0, 2);
    lcd.print("Z=");
    lcd.setCursor(3,2);
    lcd.print(z);
  
 
  float heading = atan2(y, x);
  float declinationAngle = 0.22;
  heading += declinationAngle;
  if (heading < 0) {
    heading += 2 * PI;
  }
  if (heading > 2 * PI) {
    heading -= 2 * PI;
  }
  float headingDegrees = heading * 180 / M_PI;
  
  lcd.setCursor(0, 3);
    lcd.print("Heading : ");
    lcd.setCursor(10, 3);
    // Envia valor Z do acelerômetro para o LCD
    lcd.print(headingDegrees);
    Myservo.write(headingDegrees);
   delay(1000);
}