#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27,16,2);
#include<Servo.h>

#define btnR 12
#define btnG 8
#define btnB 7
#define trucX  A1
#define trucY A0

Servo myservo[4];
int servopin[]={11,10,9,6};
int id=0;
int goc[]={0,0,0,0};




int toadoA[]={0,0,0,0};
int toadoB[]={0,0,0,0};

void setup() {

lcd.init();
lcd.backlight();
pinMode(trucX, INPUT);
pinMode(trucY, INPUT);
pinMode(btnR, INPUT_PULLUP);
pinMode(btnG, INPUT_PULLUP);
pinMode(btnB, INPUT_PULLUP);
//cau b
for(int i=0; i<4; i++){
  myservo[i].attach(servopin[i]);
  myservo[i].write(goc[i]);
}

  





}

void loop() {
  
 int X=analogRead(trucX);
 int Y = analogRead(trucY);

int state =digitalRead(btnB);
static bool press= false;
static unsigned long buttontime=0;

if(state==LOW && !press){
  press= true;
  buttontime=millis();
}

if(state==HIGH&& press){
  press=false;
}
//cau i
if(press&& (millis()-buttontime>=3000)){
  for(int i=0; i<4; i++){
    goc[i]=0;
    myservo[i].write(goc[i]);

  }
}
//cau d
if(X==1023){
  goc[id]+=10;
  myservo[0].write(goc[id]);
  myservo[1].write(goc[id]);
  for(int i=0; i<2; i++){
  toadoA[i]= goc[id];
  }
  for(int i=0; i<2; i++){
  toadoB[i]= goc[id];
  }
  
} else if(X==0){
    goc[id]-=10;
  myservo[0].write(goc[id]);
  myservo[1].write(goc[id]);
  for(int i=0; i<2; i++){
  toadoA[i]= goc[id];
  }
  for(int i=0; i<2; i++){
  toadoB[i]= goc[id];
  }
}
//cau e
if(Y==1023){
  goc[id]+=20;
  myservo[2].write(goc[id]);
  myservo[3].write(goc[id]);
   for(int i=2; i<4; i++){
  toadoA[i]= goc[id];
  }
  for(int i=2; i<4; i++){
  toadoB[i]= goc[id];
  }
} else if(Y==0){
    goc[id]-=20;
  myservo[2].write(goc[id]);
  myservo[3].write(goc[id]);
    for(int i=2; i<4; i++){
  toadoA[i]= goc[id];
  }
   for(int i=2; i<4; i++){
  toadoB[i]= goc[id];
  }
}
//cau f
if(digitalRead(btnR)==LOW){
 for(int i=0; i<4; i++){
  
  toadoA[0]=toadoA[id];

 }
}
//cau g
if(digitalRead(btnG)==LOW){
 for(int i=0; i<4; i++){
  
  toadoB[0]=toadoB[id];

 }
}
//cau h


if(press&& (millis()-buttontime<3000)){
  for(int i=0; i<4; i++){
    
    myservo[i].write(toadoA[i]);
    
    myservo[i].write(toadoB[i]);
    delay(1000);
    

  }
}


// cau c
  
lcd.setCursor(0,0);
lcd.print(" X: " );
lcd.print(X);
lcd.setCursor(10,0);
lcd.print("Y: ");
lcd.print(Y);



delay(50);

}