#include <LiquidCrystal_I2C.h> //מזמן סיפריית LCD
#include <Wire.h>
#include <Servo.h>

Servo sUD;
Servo sRL;
Servo closeL;
Servo closeR;

LiquidCrystal_I2C lcd(0x27,16,2);

int horPin=A0;
int vertPin=A1;

int i=0;

int UDtemp=0;
int LRtemp=0;

int UD=0;
int LR=0;

int pos=90;
int posLR=90;
int posC=180;

int buttonPin=8;

void setup() {
  Serial.begin(9600);
  // הגדרת מסך
    lcd.begin(16,2);
    lcd.backlight();
    lcd.setBacklight(HIGH);
  // הגדרת מנועים
    sUD.attach(3);
    sRL.attach(4);
    closeL.attach(11);
    closeR.attach(10);
    sUD.write(90);
    sRL.write(90);
    closeL.write(180);
    closeR.write(180);
  pinMode(buttonPin, INPUT); // הגדרת כפתור
  }

void loop() {
  // מקבל את התנועות שלן הסטיק
    UD=analogRead(horPin);
    LR=analogRead(vertPin);
  
  setLcd();

  // מהתנועות של הסטיק ממיר למנועים
    if (UD>512) 
      moveUp();
    if (UD<512) 
      moveDown();

    if (LR>512) 
    moveLeft();
    if (LR<512)
      moveRight();
  
  if (digitalRead(buttonPin)==HIGH) {
    close();
    Serial.println("yesssssss");
  }
  if (digitalRead(buttonPin)==LOW) {
    open();
    Serial.println("nooooo");
  }


  delay(25);


}

void moveUp() {
  sUD.write(pos);
  pos-=2;
}

void moveDown() {
  sUD.write(pos);
  pos+=2;
}

void moveLeft() {
  sRL.write(posLR);
  posLR+=2;
}

void moveRight() {
  sRL.write(posLR);
  posLR-=2;
}

void close() {
  if (posC==180) {
    for (posC=180; posC>=90; posC--) {
      closeL.write(posC);
      closeR.write(posC);
      delay(10);
    }
  }
}

void open() {
  if (posC==90) {
    for (posC=90; posC<=180; posC++) {
      closeR.write(posC);
      delay(10);
    }
    for (posC=90; posC>=0; posC--) {
      closeL.write(posC);
      delay(10);
    }
  }
}

void setLcd() {
  if (UD!=UDtemp) {
    lcd.setCursor(9,0);
    lcd.print("           ");
    lcd.setCursor(0,0);
    lcd.print("UP DOWN: ");
    lcd.print(UD);
  }

  if (LR!=LRtemp) {
    lcd.setCursor(12,1);
    lcd.print("             ");
    lcd.setCursor(0,1);
    lcd.print("LEFT RIGHT: ");
    lcd.print(LR);
  }

  UDtemp=UD;
  LRtemp=LR;
}