// Bài toán ví dụ:
// Có 6 Servo góc xoay từ 0 đến 180, 1 Joystick, 1 Button và 1 LCD
// Khi bấm nút(ở giữa) Joystick sẽ thay đổi Servo điều khiển
// Joystick: nhấn lên servo được chọn tăng 1 độ, và ngược lại giảm một
// Joystick: nhấn trái tăng 3 độ, và ngược lại
// Button: được bấm sẽ reset tất cả về 0 độ
// LCD hiển thị Servo đg chọn và góc của servo đó

//Khai báo I2C
#include <LiquidCrystal_I2C.h>
//16 dòng 2 cột
LiquidCrystal_I2C lcd(0x27, 16, 2);

//Khai báo Joystick và button Reset
#define TrucNgang A0
#define TrucDoc A1
#define NutJoy 2 //chân nút chuyển servo
#define NutReset 7 //Chân nút reset

//Khai báo cho servo
#include <Servo.h>
Servo myservo[6]; //Khai báo mãng servo
int ServoPin[]={3,5,6,9,10,11}; //Khai báo các chân servo
int IdxServo=0; //con servo đc được chọn vd=0 tức con có chân tại ServoPin[0] tức 3 đc được chọn
int Goc[]={0,0,0,0,0,0}; //Khai báo góc của các servo

void setup() {
  //setup cho lcd
  lcd.init();
  lcd.backlight();
  lcd.print("Servo: 0");
  lcd.setCursor(0, 1);
  lcd.print("Goc: 0");

  //Setup các nút joystick
  pinMode(TrucNgang, INPUT);
  pinMode(TrucDoc, INPUT);
  pinMode(NutJoy, INPUT_PULLUP);
  pinMode(NutReset, INPUT_PULLUP);

  //bật serial để xem thích thì xem ko thì xóa
  Serial.begin(9600);

  //khai báo cho các servo và set tọa độ về 0
  for(int i=0;i<6;i++){
    myservo[i].attach(ServoPin[i]);
    myservo[i].write(Goc[i]);
  }
}

void loop() {

  //khi bấm nút ở giữ sẽ chuyển đổi sang con servo khác
  if(digitalRead(NutJoy) == LOW){
    IdxServo += 1;
    IdxServo = IdxServo % 6; //Này để set cho nó không hơn 6 con á (._.`)
  }

  //đọc trục ngang, dọc của joystick
  int TNgang = analogRead(TrucNgang);
  int TDoc = analogRead(TrucDoc);
  // Trục ngang từ 0 (phải) tới 1023 (trái)
  // Trục dọc từ 0 (dưới) tới 1023 (trên)
  // Khi không bấm gì thì tọa độ sẽ là ngang 512 và dọc là 512
  
  if(TDoc == 1023){ //Nếu lên thì tăng 1
    Goc[IdxServo] += 1;
  }else if(TDoc == 0){ //Nếu xuống thì giảm 1
    Goc[IdxServo] -= 1;
  }
  if(TNgang == 1023){ //Nếu trái thì tăng 3
    Goc[IdxServo] += 3;
  }else if(TNgang == 0){ //Nếu phải thì giảm 3
    Goc[IdxServo] -= 3;
  }

  //Kiểm tra xem có quá giới hạn từ 0-180 không
  if(Goc[IdxServo]>180){ //Nếu trên 180 thì cho về 180
    Goc[IdxServo]=180;
  }else if(Goc[IdxServo]<0){ //Nếu dưới 0 thì cho về 0
     Goc[IdxServo]=0;
  }

  //Cho con servo hiện tại đang chọn quay đến góc của nó
  myservo[IdxServo].write(Goc[IdxServo]);

  //Nếu bấm nút thì các góc sẽ được set về 0 và cả những con servo nữa
  if(digitalRead(NutReset) == LOW){
    for(int i=0;i<6;i++){
      Goc[i]=0;
      myservo[i].write(Goc[i]);
    }
  }

  //Hiển thị Servo đg chọn và góc của servo đó
  lcd.setCursor(7, 0);
  lcd.print(IdxServo+1); //Servo đg chọn
  lcd.setCursor(5, 1);
  lcd.print(Goc[IdxServo]); //Góc của servo
  //Trick lỏ fix lỗi hiển thị
  lcd.print("  ");


  //Này hiển thị servo đg chọn mún thì để nếu trên xóa mà dưới còn
  // là đỏ chót (ㆆ_ㆆ)!
  Serial.print("Servo đang chọn: ");
  Serial.print(IdxServo);
  Serial.println();


  //Phải có delay để không nhanh quá
  delay(50);
}