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

#define PIN_VRx A0
#define PIN_VRy A1
#define PIN_SW 6
#define PIN_SERVO 7

Servo motor;
int x, y;
int x_ang, y_ang;

LiquidCrystal_I2C lcd (0x27, 16, 2);

int Led1 = 2;
int Led2 = 3;
int Led3 = 4;
int Led4 = 5;

void setup() {

  Serial.begin(9600);
 
  pinMode( PIN_SW, INPUT_PULLUP );
  pinMode( Led1, OUTPUT );
  pinMode( Led2, OUTPUT );
  pinMode( Led3, OUTPUT );
  pinMode( Led4, OUTPUT );

  motor.attach(PIN_SERVO);
  motor.write(0);

  lcd.init();
  lcd.backlight();

}

void loop() {

  motor.write(0);

  x = analogRead(PIN_VRx);
  y = analogRead(PIN_VRy);

  x_ang = map( x, 0, 1023, 0, 180 );
  y_ang = map( y, 0, 1023, 0, 180 );

  Serial.print( "X_ang:" );
  Serial.print(x_ang);
  Serial.print("  ");

  Serial.print( "Y_ang:" );
  Serial.print(y_ang);
  Serial.print( " SW:");
  Serial.print( digitalRead(PIN_SW) );
  Serial.println();

  if (x_ang == 180){
    lcd.print("Izquieda");
    digitalWrite(Led2, HIGH);
    delay(500);
    digitalWrite(Led2, LOW);
    delay(1000);
    lcd.clear();
  }

  if (x_ang == 0){
    lcd.print("Derecha");
    digitalWrite(Led3, HIGH);
    delay(500);
    digitalWrite(Led3, LOW);
    delay(1000);
    lcd.clear();
  }

  if (y_ang == 180){
    lcd.print("Arriba");
    digitalWrite(Led1, HIGH);
    delay(500);
    digitalWrite(Led1, LOW);
    delay(1000);
    lcd.clear();
  }

  if (y_ang == 0){
    lcd.print("Abajo");
    digitalWrite(Led4, HIGH);
    delay(500);
    digitalWrite(Led4, LOW);
    delay(1000);
    lcd.clear();
  }

  if (digitalRead(PIN_SW) == 0){
    lcd.print("Boton");
    motor.write(180);
    delay(1000);
    lcd.clear();
  }
  
  delay(250);
}