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

LiquidCrystal_I2C lcd(0x27,20,04);

Servo myservo[4];
int servopin[4]={3,5,6,9};
int servoangle[4]={0,0,0,0};

long lasttime=0;
bool ispress=false;

#define mybutton 7
#define vert A0
#define horz A1

void setup() {
  lcd.init();
  lcd.backlight();

  pinMode(mybutton, INPUT_PULLUP);
  pinMode(vert, INPUT);
  pinMode(horz, INPUT);

  lcd.setCursor(5,0);
  lcd.print("X:");
  lcd.setCursor(5,1);
  lcd.print("Y:");

  for(int i=0;i<4;i++){
    myservo[i].attach(servopin[i]);
    myservo[i].write(0);
  }
}

void loop() {
  if(!digitalRead(mybutton)){
    if(ispress && millis()-lasttime>=3000){
      for(int i=0;i<4;i++){
        servoangle[i]=180;
        myservo[i].write(servoangle[i]);
      }
    }else if(!ispress){
      ispress=true;
      lasttime=millis();
    }
  }else{
    ispress=false;
  }

  int mvert=analogRead(vert);// Trục Y
  int mhorz=analogRead(horz);// trục X
  if(mhorz==1023){
    servoangle[0]+=1;
    servoangle[1]-=1;
    myservo[0].write(servoangle[0]);
    myservo[1].write(servoangle[1]);
  }else if(mhorz==0){
    servoangle[0]-=1;
    servoangle[1]+=1;
    myservo[0].write(servoangle[0]);
    myservo[1].write(servoangle[1]);
  }

  if(mvert==1023){
    servoangle[2]-=1;
    servoangle[3]-=1;
    myservo[2].write(servoangle[2]);
    myservo[3].write(servoangle[3]);
  }else if(mvert==0){
    servoangle[2]+=1;
    servoangle[3]+=1;
    myservo[2].write(servoangle[2]);
    myservo[3].write(servoangle[3]);
  }
  for(int i=0;i<4;i++){
    if(servoangle[i]<0){
      servoangle[i]=0;
    }
    if(servoangle[i]>180){
      servoangle[i]=180;
    }
  }

  lcd.setCursor(7,0);
  lcd.print(mhorz);
  lcd.print("   ");
  lcd.setCursor(7,1);
  lcd.print(mvert);
  lcd.print("   ");
  delay(50);
}