#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <MPU6050.h>
#include <Servo.h>

MPU6050 mpu;
LiquidCrystal_I2C lcd(0x27, 16, 2); // Dirección I2C y tamaño (16x2)
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

void setup() {
  Wire.begin();
  mpu.initialize();
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  servo1.attach(9);  // Asigna el servo 1 al pin 9
  servo2.attach(10); // Asigna el servo 2 al pin 10
  servo3.attach(11); // Asigna el servo 3 al pin 11
  servo4.attach(12); // Asigna el servo 4 al pin 12
}

void loop() {
  int16_t ax, ay, az, gx, gy, gz;
  float pitch, roll;
  
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Calcula los ángulos de inclinación en los ejes x e y
  pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / M_PI;
  roll = atan2(ay, az) * 180.0 / M_PI;

  // Calcula los ángulos de dirección (norte/sur/este/oeste)
  float northAngle = map(pitch, -90, 90, 0, 180);
  float southAngle = 180 - northAngle;
  float eastAngle = map(roll, -90, 90, 0, 180);
  float westAngle = 180 - eastAngle;

  // Controla los servos en función de los ángulos calculados
  servo1.write(northAngle);
  servo2.write(southAngle);
  servo3.write(eastAngle);
  servo4.write(westAngle);

  // Muestra los ángulos de inclinación en la pantalla LCD
  lcd.setCursor(0, 0);
  lcd.print("N:");
  lcd.print(pitch, 1);
  lcd.print(" S:");
  lcd.print(180 - pitch, 1);
  lcd.setCursor(0, 1);
  lcd.print("E:");
  lcd.print(roll, 1);
  lcd.print(" W:");
  lcd.print(180 - roll, 1);

  delay(500); // Actualiza cada 500 ms
}