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

Servo servoA; // Op/Neer
Servo servoB; // Vooruit/achteruit

#define VERT_PIN1 A0
#define HORZ_PIN1 A1
#define SEL_PIN1  2

#define REDLED 6

#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

void setup() {
  pinMode(VERT_PIN1, INPUT);
  pinMode(HORZ_PIN1, INPUT);
  pinMode(SEL_PIN1, INPUT_PULLUP);

  pinMode(REDLED, OUTPUT);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);  
  lcd.print("Joystick servo drive");
  servoA.attach(4);
  servoB.attach(5);
  servoA.write(0);
  servoB.write(0);
  delay(2000);
}

void loop() {
  int vert1 = analogRead(VERT_PIN1);
  int horz1 = analogRead(HORZ_PIN1);
  bool selPressed1 = digitalRead(SEL_PIN1) == LOW;

  // horz goes from 0 (right) to 1023 (left)
  // vert goes from 0 (bottom) to 1023 (top)
  // selPressed is true is the joystick is pressed

  lcd.setCursor(2,1);
  lcd.print("x:");
  lcd.setCursor(4,1);
  lcd.print(horz1);
  lcd.setCursor(9,1);
  lcd.print("y:");
  lcd.print(vert1);
  lcd.setCursor(16,1);
  lcd.print(selPressed1);
  digitalWrite(REDLED,selPressed1);
  servoA.write((1024-vert1)/5.68);
  servoB.write((1024-horz1)/5.68);

  delay(1000);
  lcd.clear();
  lcd.setCursor(2, 0);  
  lcd.print("Double joystick");
}