// Library
#include <Keypad.h>
#include <LiquidCrystal_I2C.h>
#include <Joystick.h>

// Changelog
const String updated_Version = "New_mode-V2";
const String updated_Date = "2024/11/01";

// 更新日誌：
// #當前藍牙模塊(HC-05)尚未安裝(for 10/03)。
// #當前線上/線下模式切換按鍵尚未啟用(for 10/3)。
// #線上模式程式尚未完成(for 10/3)。
// #模式T改動(for 10/3)
// #模式T優化(for 10/27)
// #線上模式A暫時完成(缺再次輸入)

// 問題修復：
// #模式E執行完重複精度測試後 換置模式D時被歸零 且B紀錄點位沒被記錄至E(fix on 09/09)

// Pin set...
// Button Pin(joy: 搖桿、limit:限位開關)
const int joyX = A6;
const int joyY = A7;
const int joyButton = 50;
const int xlimitPin = 52;
const int ylimitPin = 12;
const int zlimitPin = 13;

// RGB LED
const int R_LED = 14;
const int G_LED = 15;
const int B_LED = 16;

// Stepper motor Pin(en:致能、step:步、dir:轉向)
const int enablePin = 8;
const int xstepPin = 2;
const int ystepPin = 3;
const int zstepPin = 4;
const int xdirPin = 5;
const int ydirPin = 6;
const int zdirPin = 7;

// Array set...
// Keypad set
const byte ROWS = 6;        // 行(橫)
const byte COLS = 4;        // 列(豎)
char keys[ROWS][COLS] = {
  {'s', ' ', ' ', ' '},     // 's':確認/鎖定
  {'<', ' ', ' ', ' '},     // '<':左移/減
  {'>', ' ', ' ', ' '},     // '>':右移/增
  {' ', 'o', ' ', ' '},     // 'o':馬達啟動/繼續
  {' ', ' ', 'm', ' '},     // 'm':模式選擇/返回
  {' ', ' ', ' ', 'p'}      // 'p':線上/線下切換
};
byte rowPins[ROWS] = {A12, A13, A14, A8, A10, 23};
byte colPins[COLS] = {A15, A9, A11, 25};

// list set
const int B_listSize = 10;  // 陣列大小(mode_B, E)
const int D_listSize = 10;  // 陣列大小(mode_D)
long B_x_axis[B_listSize];   // X軸點位存儲(mode_B)
long B_y_axis[B_listSize];   // Y軸點位存儲(mode_B)
long D_x_axis[D_listSize];   // X軸點位存儲(mode_D)
long D_y_axis[D_listSize];   // Y軸點位存儲(mode_D)
long D_x_axis_01[B_listSize];// X軸點位存儲(mode_D)
long D_y_axis_01[B_listSize];// Y軸點位存儲(mode_D)
long E_x_axis[B_listSize];   // X軸點位存儲(mode_E)
long E_y_axis[B_listSize];   // Y軸點位存儲(mode_E)

// Global variables...
// Judgment with variables(判斷用變數)
int modeChange = 0;         // 模式變更
int numlocation = 0;        // 變更數字位置(mode_A, C, D)
int button = 0;             // 搖桿按鈕(mode_B)

bool modeReturn = false;    // 模式返回
bool modeStart = false;     // 馬達啟用
bool D_modeStart = false;   // 馬達啟用(mode_D)
bool online = false;        // 線上/線下
bool xlimit = false;
bool ylimit = false;
bool zlimit = false;
bool xlimitJudge = false;   // Y軸限位狀態
bool ylimitJudge = false;   // Y軸限位狀態
bool zlimitJudge = false;   // Z軸限位狀態

// mode variables(模式變數)
bool A_Correction = false;
bool B_Manual = false;
bool C_Semi_automatic = false;
bool D_Automatic = false;
bool E_Point_custom = false;
int T_Developer = 0;
bool Again = false;

// Numerical storage(數值存儲)
// General
char words;
long x_coordinate = 0;      // X軸座標
long y_coordinate = 0;      // Y軸座標
long xmoveSteps = 0;        // X軸步數
long ymoveSteps = 0;        // Y軸步數
char xsymbol = '.';
char ysymbol = '.';
int xnum_01 = 0;
int xnum_02 = 0;
int xnum_03 = 0;
int xnum_04 = 0;
int ynum_01 = 0;
int ynum_02 = 0;
int ynum_03 = 0;
int ynum_04 = 0;
int delaytime = 0;          // 延遲時間(mode_E)

// mode_A
int Anum_01 = 1;
int Anum_02 = 0;
int Anum_03 = 0;

// mode_B
int B_i = 1;
bool B_display = false;     // 状态面板显示

// mode_D
int D_i = 0;                // list使用
int Dnum_01 = 0;
int Dnum_02 = 0;
int Dnum_03 = 0;

// mode_E
int Enum_01 = 1;            // 累計執行次數
int Enum_02 = 0;            // 存儲秒數(十位數)
int Enum_03 = 0;            // 存儲秒數(個位數)
int Enum_04 = 0;            // 重複執行存儲次數
int Enum_05 = 0;            // 延遲時間存儲

// mode_T
int T_mode = 0;             // 模式T參數
int limitenable = 0;        // 限位模式參數
int delayenable = 0;        // 延遲參數
int motorDelay = 400;       // 步數延遲

// set Library module
Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );
Joystick myJoystick(joyX, joyY, joyButton);
LiquidCrystal_I2C lcd(0x27, 20, 4);

// Initial setup(初始設定)
void setup() {
  // set motor Pin
  pinMode(enablePin, OUTPUT);

  pinMode(xdirPin, OUTPUT);
  pinMode(ydirPin, OUTPUT);
  pinMode(zdirPin, OUTPUT);

  pinMode(xstepPin, OUTPUT);
  pinMode(ystepPin, OUTPUT);
  pinMode(zstepPin, OUTPUT);

  digitalWrite(enablePin, LOW);

  // set RGB LED
  pinMode(B_LED, OUTPUT);
  pinMode(G_LED, OUTPUT);
  pinMode(R_LED, OUTPUT);

  digitalWrite(B_LED, LOW);
  digitalWrite(G_LED, LOW);
  digitalWrite(R_LED, HIGH);

  // set Joystick
  pinMode(joyX, INPUT);
  pinMode(joyX, INPUT);
  pinMode(joyButton, INPUT);

  // set limit
  pinMode(xlimitPin, INPUT);
  pinMode(ylimitPin, INPUT);
  pinMode(zlimitPin, INPUT);

  // set LCD
  lcd.init();
  lcd.backlight();

  // begin
  Serial.begin(9600);
  Serial1.begin(9600);

  lcd.setCursor(0, 0);
  lcd.print("Welcome using,");
  //lcd.print("Welcome using,");
  lcd.setCursor(17, 0);
  lcd.print("O:");
  lcd.setCursor(19, 0);
  lcd.print("L");
  lcd.setCursor(0, 1);
  lcd.print("> mode_A <");
  lcd.setCursor(2, 2);
  lcd.print("mode_B");
  lcd.setCursor(12, 1);
  lcd.print("mode_C");
  lcd.setCursor(12, 2);
  lcd.print("mode_D");
  lcd.setCursor(6, 3);
  lcd.print("|");
  lcd.setCursor(8, 3);
  lcd.print("mode");
  lcd.setCursor(13, 3);
  lcd.print("|");
  lcd.setCursor(14, 3);
  lcd.print("online");
  // 初始状态(串口)显示
  Serial.println("# 此為線上模式專用串口通信，請輸入字母以進入模式(A~V):");
  Serial1.println("# 此為線上模式專用串口通信，請輸入字母以進入模式(A~V):");
}

void loop() {
  // Arduino窗口讀取
  if (Serial.available() > 0) {
    // 模式讀取
    char modeChoose = Serial.read();
    Serial.print("- 當前使用的串口為\'Serial port-0\'，當前模式選擇為:");
    Serial.println(modeChoose);
    mode_online(modeChoose);
  } else if (Serial1.available() > 0) {
    // 模式讀取
    char modeChoose = Serial1.read();
    Serial1.print("- 當前使用的串口為\'Serial port-1\'，當前模式選擇為:");
    Serial1.println(modeChoose);
    mode_online(modeChoose);
  }

  // 實體按鍵讀取
  char keys = keypad.getKey();
  if (keys != NO_KEY) {
    mode_Offline(keys);
  }
}

// LCD_Display：LCD顯示(Rows：列，Cols：行，str：文字內容)
void LCD_Display(int Rows, int Cols, String str) {
  lcd.setCursor(Rows, Cols);
  lcd.print(str);
}

// 模式(線上)
void mode_online(char modeChoose) {
  switch (modeChoose) {
    case 'A':
      online = true;
      mode_A();
      mode_Offline('<');
      online = false;
      break;
    case 'B':
      online = true;
      mode_B();
      online = false;
      break;
    case 'C':
      online = true;
      mode_C();
      mode_Offline('<');
      online = false;
      break;
    case 'D':
      online = true;
      mode_D();
      mode_Offline('<');
      online = false;
      break;
    case 'E':
      online = true;
      mode_E();
      online = false;
      break;
  }
}

// Correction_procedure：歸零(frequency：校正次數)(for 10/27)
void Correction_procedure(int frequency) {
  // 測試模式下不進行校正，模式A不受影響
  if (T_Developer == 0 || A_Correction == true) {
    digitalWrite(B_LED, HIGH);
    digitalWrite(R_LED, LOW);
    while (frequency > 0) {
      if (!xlimitJudge && digitalRead(xlimitPin) == 0) {
        digitalWrite(xstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(xstepPin, LOW);
        delayMicroseconds(10);
      } else if (digitalRead(xlimitPin) == 1 && xlimit == false) {
        xlimitJudge = true;
        xlimit = true;
        Serial.println("~ Xstop已觸發");
        Serial1.println("~ Xstop已觸發");
      }

      if (!ylimitJudge && digitalRead(ylimitPin) == 0) {
        digitalWrite(ystepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(ystepPin, LOW);
        delayMicroseconds(10);
      } else if (digitalRead(ylimitPin) == 1 && ylimit == false) {
        ylimitJudge = true;
        ylimit = true;
        Serial.println("~ Ystop已觸發");
        Serial1.println("~ Ystop已觸發");
      }

      if (!zlimitJudge && digitalRead(zlimitPin) == 0) {
        digitalWrite(zstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(zstepPin, LOW);
        delayMicroseconds(10);
      } else if (digitalRead(zlimitPin) == 1 && zlimit == false) {
        zlimitJudge = true;
        zlimit = true;
        Serial.println("~ Zstop已觸發");
        Serial1.println("~ Zstop已觸發");
      }

      if (xlimitJudge && ylimitJudge && zlimitJudge) {
        digitalWrite(xdirPin, HIGH);
        digitalWrite(ydirPin, HIGH);
        digitalWrite(zdirPin, HIGH);
        for (int i = 0; i < 2500; i++) {
          digitalWrite(xstepPin, HIGH);
          digitalWrite(ystepPin, HIGH);
          digitalWrite(zstepPin, HIGH);
          delayMicroseconds(10);
          digitalWrite(zstepPin, LOW);
          digitalWrite(ystepPin, LOW);
          digitalWrite(xstepPin, LOW);
          delayMicroseconds(10);
          // 每步間隔
          delayMicroseconds(motorDelay);
        }
        xlimitJudge = false;
        ylimitJudge = false;
        zlimitJudge = false;

        xlimit = false;
        ylimit = false;
        zlimit = false;
        digitalWrite(xdirPin, LOW);
        digitalWrite(ydirPin, LOW);
        digitalWrite(zdirPin, LOW);
        frequency--;
        if (frequency == 0)
          break;
      }
      delayMicroseconds(motorDelay);
    }
    digitalWrite(B_LED, LOW);
    digitalWrite(R_LED, HIGH);
  }
  // 測試模式僅xlimit校正(for 10/27)
  else if (T_Developer == 2) {
    digitalWrite(B_LED, HIGH);
    digitalWrite(R_LED, LOW);
    while (frequency > 0) {
      if (!xlimitJudge && digitalRead(xlimitPin) == 0) {
        digitalWrite(xstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(xstepPin, LOW);
        delayMicroseconds(10);
      } else if (digitalRead(xlimitPin) == 1 && xlimit == false) {
        xlimitJudge = true;
        xlimit = true;
        Serial1.println("Xstop");
      }
      if (xlimitJudge) {
        digitalWrite(xdirPin, HIGH);
        for (int i = 0; i < 2500; i++) {
          digitalWrite(xstepPin, HIGH);
          delayMicroseconds(10);
          digitalWrite(xstepPin, LOW);
          delayMicroseconds(10);
          // 每步間隔
          delayMicroseconds(motorDelay);
        }
        xlimitJudge = false;
        xlimit = false;
        digitalWrite(xdirPin, LOW);
        frequency--;
        if (frequency == 0)
          break;
      }
      delayMicroseconds(motorDelay);
    }
    digitalWrite(B_LED, LOW);
    digitalWrite(R_LED, HIGH);
  }
  // 測試模式僅y-zlimit校正(for 10/27)
  else if (T_Developer == 3) {
    digitalWrite(B_LED, HIGH);
    digitalWrite(R_LED, LOW);
    while (frequency > 0) {
      if (!ylimitJudge && digitalRead(ylimitPin) == 0) {
        digitalWrite(ystepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(ystepPin, LOW);
        delayMicroseconds(10);
      } else if (digitalRead(ylimitPin) == 1 && ylimit == false) {
        ylimitJudge = true;
        ylimit = true;
        Serial1.println("Ystop");
      }
      if (!zlimitJudge && digitalRead(zlimitPin) == 0) {
        digitalWrite(zstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(zstepPin, LOW);
        delayMicroseconds(10);
      } else if (digitalRead(zlimitPin) == 1 && zlimit == false) {
        zlimitJudge = true;
        zlimit = true;
        Serial1.println("Zstop");
      }
      if (ylimitJudge && zlimitJudge) {
        digitalWrite(ydirPin, HIGH);
        digitalWrite(zdirPin, HIGH);
        for (int i = 0; i < 2500; i++) {
          digitalWrite(ystepPin, HIGH);
          digitalWrite(zstepPin, HIGH);
          delayMicroseconds(10);
          digitalWrite(ystepPin, HIGH);
          digitalWrite(zstepPin, HIGH);
          delayMicroseconds(10);
          // 每步間隔
          delayMicroseconds(motorDelay);
        }
        ylimitJudge = false;
        zlimitJudge = false;
        ylimit = false;
        zlimit = false;
        digitalWrite(ydirPin, LOW);
        digitalWrite(zdirPin, LOW);
        frequency--;
        if (frequency == 0)
          break;
      }
      delayMicroseconds(motorDelay);
    }
    digitalWrite(B_LED, LOW);
    digitalWrite(R_LED, HIGH);
  }
  Serial.println("- 馬達已校正完成!");
  Serial1.println("- 馬達已校正完成!");
  // 狀態顯示(for 10/28)
  if (A_Correction == true) {
    Serial.println("- 請輸入大寫字母'R(返回)'以繼續!");
    Serial1.println("- 請輸入大寫字母'R(返回)'以繼續!");
  }
}

// 馬達啟動，通用副程式，以判斷模式為基礎
void motor_ON() {
  // mode_A，校正模式...
  while (A_Correction == true) {
    char keys = keypad.getKey();
    if (Serial.available() > 0)
      words = Serial.read();
    else if (Serial1.available() > 0)
      words = Serial1.read();
    // 校正程式(校正次數Anum_02，已校正次數累計顯示Anum_03，當前狀態(燈號、訊息))...
    while (Anum_01 <= Anum_02) {
      // 當前狀態顯示
      digitalWrite(B_LED, HIGH);
      digitalWrite(R_LED, LOW);
      lcd.setCursor(0, 2);
      lcd.print("state: moving ");
      lcd.print(Anum_01);
      // set motor_dir = LOW
      digitalWrite(xdirPin, LOW);
      digitalWrite(ydirPin, LOW);
      digitalWrite(zdirPin, LOW);
      // input motor while
      Correction_procedure(1);
      delay(500);
      Anum_01++;
    }
    // 儲存數值並顯示
    digitalWrite(B_LED, LOW);
    digitalWrite(R_LED, HIGH);
    lcd.setCursor(0, 1);
    lcd.print("Current total: ");
    lcd.print(Anum_03);
    lcd.setCursor(0, 2);
    lcd.print("state: finish ");
    lcd.setCursor(8, 0);
    lcd.print("finish");
    modeLCD_display("", 2, 1, online, "      ", "return", "      ");

    // 判斷是否按下'm'
    if (keys == 'm' || words == 'R') {
      lcd.setCursor(0, 0);
      lcd.print("mode_A: return");
      modeLCD_display("", 2, 1, online, "      ", "  OK  ", "      ");
      Serial.println("# 已退出模式A!");
      Serial1.println("# 已退出模式A!");
      Anum_01 = 1;
      A_Correction = false;
      D_Automatic = false;
      modeStart = false;
      break;
    }

    if (keys == 's') {
      Anum_01 = 1;
      Serial1.println("press A~D choose mode");
      A_Correction = false;
      // 原因不明
      D_Automatic = false;
      modeStart = false;
      // mode_A();
      break;
    }
  }
  // mode_B，手動(搖桿)模式
  while (B_Manual == true) {
    // 讀取搖桿數值
    if (Serial.available() > 0)
      words = Serial.read();
    else if (Serial1.available() > 0)
      words = Serial1.read();
    int xValue = analogRead(joyX);
    int yValue = analogRead(joyY);
    // 搖桿數值顯示(嚴重影響馬達運作，暫時註解)
    /*lcd.setCursor(3, 2);
      lcd.print("X: ");
      lcd.print(xValue);
      lcd.setCursor(11, 2);
      lcd.print("Y: ");
      lcd.print(yValue);
      delay(250);
      lcd.setCursor(3, 2);
      lcd.print("X:           ");
      lcd.setCursor(11, 2);
      lcd.print("Y:          ");*/
    // 判斷搖桿幅度。如果搖桿幅度過小，跳出副程式。
    if ((400 < xValue && xValue < 800) && (300 < yValue && yValue < 800) && online == false || online == true && words == 'R') {
      //Serial.println("YG_P");
      Serial.println(x_coordinate);
      Serial.println(y_coordinate);
      Serial.println("===========");
      digitalWrite(G_LED, LOW);
      digitalWrite(R_LED, HIGH);
      lcd.setCursor(8, 0);
      lcd.print("start");
      xsymbol = '.';
      ysymbol = '.';
      B_Manual = false;
      //online = false;
      break;
    }
    // 馬達運作
    if (xsymbol == '+' || words == '4') {
      // X軸馬達正轉...
      //Serial.println("X軸+");
      digitalWrite(xstepPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(xstepPin, LOW);
      delayMicroseconds(10);
      x_coordinate++;
    } else if (xsymbol == '-' || words == '6') {
      // X軸馬達反轉...
      //Serial.println("X軸-");
      digitalWrite(xstepPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(xstepPin, LOW);
      delayMicroseconds(10);
      x_coordinate--;
    } else if (ysymbol == '+' || words == '2') {
      // Y軸馬達正轉...
      //Serial.println("Y軸+");
      digitalWrite(ystepPin, HIGH);
      digitalWrite(zstepPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(ystepPin, LOW);
      digitalWrite(zstepPin, LOW);
      delayMicroseconds(10);
      y_coordinate++;
    } else if (ysymbol == '-' || words == '8') {
      // Y軸馬達反轉...
      //Serial.println("Y軸-");
      digitalWrite(ystepPin, HIGH);
      digitalWrite(zstepPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(ystepPin, LOW);
      digitalWrite(zstepPin, LOW);
      delayMicroseconds(10);
      y_coordinate--;
    } else if (xsymbol == '@' || words == '7') {
      // X、Y軸馬達正、反轉...
      //Serial.println("X、Y軸馬達正、反轉");
      digitalWrite(xstepPin, HIGH);
      digitalWrite(ystepPin, HIGH);
      digitalWrite(zstepPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(xstepPin, LOW);
      digitalWrite(ystepPin, LOW);
      digitalWrite(zstepPin, LOW);
      delayMicroseconds(10);
      x_coordinate++;
      y_coordinate--;
    } else if (ysymbol == '#' || words == '9') {
      // X、Y軸馬達反轉...
      //Serial.println("X、Y軸馬達反轉");
      digitalWrite(xstepPin, HIGH);
      digitalWrite(ystepPin, HIGH);
      digitalWrite(zstepPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(xstepPin, LOW);
      digitalWrite(ystepPin, LOW);
      digitalWrite(zstepPin, LOW);
      delayMicroseconds(10);
      x_coordinate--;
      y_coordinate--;
    } else if (xsymbol == '$' || words == '1') {
      // X、Y軸馬達正轉...
      //Serial.println("X、Y軸馬達正轉");
      digitalWrite(xstepPin, HIGH);
      digitalWrite(ystepPin, HIGH);
      digitalWrite(zstepPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(xstepPin, LOW);
      digitalWrite(ystepPin, LOW);
      digitalWrite(zstepPin, LOW);
      delayMicroseconds(10);
      x_coordinate++;
      y_coordinate++;
    } else if (ysymbol == '%' || words == '3') {
      // X、Y軸馬達反、正轉...
      //Serial.println("X、Y軸馬達反、正轉");
      digitalWrite(xstepPin, HIGH);
      digitalWrite(ystepPin, HIGH);
      digitalWrite(zstepPin, HIGH);
      delayMicroseconds(10);
      digitalWrite(xstepPin, LOW);
      digitalWrite(ystepPin, LOW);
      digitalWrite(zstepPin, LOW);
      delayMicroseconds(10);
      x_coordinate--;
      y_coordinate++;
    }
  }
  // mode_C，半自動(數值)模式
  while (C_Semi_automatic == true) {
    char keys = keypad.getKey();
    char words = Serial1.read();
    // 狀態顯示(移動中(綠)，print: move)
    digitalWrite(G_LED, HIGH);
    digitalWrite(R_LED, LOW);
    lcd.setCursor(8, 0);
    lcd.print("start   ");
    // 馬達根據數值運作(xmoveSteps, ymoveSteps)...
    while (xmoveSteps != 0 || ymoveSteps != 0) {
      if (xmoveSteps > 0) {
        digitalWrite(xdirPin, HIGH);
        digitalWrite(xstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(xstepPin, LOW);
        delayMicroseconds(10);
        xmoveSteps--;
      }

      if (ymoveSteps > 0) {
        digitalWrite(ydirPin, HIGH);
        digitalWrite(zdirPin, HIGH);
        digitalWrite(ystepPin, HIGH);
        digitalWrite(zstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(ystepPin, LOW);
        digitalWrite(zstepPin, LOW);
        delayMicroseconds(10);
        ymoveSteps--;
      }

      if (xmoveSteps < 0) {
        digitalWrite(xdirPin, LOW);
        digitalWrite(xstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(xstepPin, LOW);
        delayMicroseconds(10);
        xmoveSteps++;
      }

      if (ymoveSteps < 0) {
        digitalWrite(ydirPin, LOW);
        digitalWrite(zdirPin, LOW);
        digitalWrite(ystepPin, HIGH);
        digitalWrite(zstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(ystepPin, LOW);
        digitalWrite(zstepPin, LOW);
        delayMicroseconds(10);
        ymoveSteps++;
      }
      //Serial.println(xmoveSteps);
      //Serial.println(ymoveSteps);
      delayMicroseconds(motorDelay);
    }
    break;
  }
  // mode_D，全自動模式
  while (D_Automatic == true) {
    char keys = keypad.getKey();
    char words = Serial1.read();
    digitalWrite(B_LED, LOW);
    digitalWrite(R_LED, HIGH);

    // 先移動到mode_B已鎖定點位
    while (D_x_axis[0] != 0 || D_y_axis[0] != 0) {
      if (D_x_axis[0] > 0) {
        digitalWrite(xdirPin, HIGH);
        digitalWrite(xstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(xstepPin, LOW);
        delayMicroseconds(10);
        D_x_axis[0]--;
      }
      if (D_y_axis[0] > 0) {
        digitalWrite(ydirPin, HIGH);
        digitalWrite(zdirPin, HIGH);
        digitalWrite(ystepPin, HIGH);
        digitalWrite(zstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(ystepPin, LOW);
        digitalWrite(zstepPin, LOW);
        delayMicroseconds(10);
        D_y_axis[0]--;
      }

      if (D_x_axis[0] < 0) {
        digitalWrite(xdirPin, LOW);
        digitalWrite(xstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(xstepPin, LOW);
        delayMicroseconds(10);
        D_x_axis[0]++;
      }

      if (D_y_axis[0] < 0) {
        digitalWrite(ydirPin, LOW);
        digitalWrite(zdirPin, LOW);
        digitalWrite(ystepPin, HIGH);
        digitalWrite(zstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(ystepPin, LOW);
        digitalWrite(zstepPin, LOW);
        delayMicroseconds(10);
        D_y_axis[0]++;
      }
      //Serial.println(D_x_axis[0]);
      //Serial.println(D_y_axis[0]);
      delayMicroseconds(motorDelay);
    }
    lcd.setCursor(8, 0);
    lcd.print("press's'");
    modeLCD_display("", 2, 1, online, "      ", "      ", "      ");
    if ((keys == 's' && D_i <= Dnum_02 + 1) || (words == '5' && D_i <= Dnum_02 + 1)) {
      D_i++;
      lcd.setCursor(8, 0);
      lcd.print("moveing");
      digitalWrite(G_LED, HIGH);
      digitalWrite(R_LED, LOW);
      /*Serial.print("Point ");
        Serial.print(D_i);
        Serial.print(": (");
        Serial.print(D_x_axis[D_i]);
        Serial.print(", ");
        Serial.print(D_y_axis[D_i]);
        Serial.println(")");
        lcd.setCursor(0, 3);*/

      // 馬達根據數值運作(xmoveSteps, ymoveSteps)...
      while (D_x_axis[D_i] != 0 || D_y_axis[D_i] != 0) {
        if (D_x_axis[D_i] > 0) {
          digitalWrite(xdirPin, HIGH);
          digitalWrite(xstepPin, HIGH);
          delayMicroseconds(10);
          digitalWrite(xstepPin, LOW);
          delayMicroseconds(10);
          D_x_axis[D_i]--;
        }

        if (D_y_axis[D_i] > 0) {
          digitalWrite(ydirPin, HIGH);
          digitalWrite(zdirPin, HIGH);
          digitalWrite(ystepPin, HIGH);
          digitalWrite(zstepPin, HIGH);
          delayMicroseconds(10);
          digitalWrite(ystepPin, LOW);
          digitalWrite(zstepPin, LOW);
          delayMicroseconds(10);
          D_y_axis[D_i]--;
        }

        if (D_x_axis[D_i] < 0) {
          digitalWrite(xdirPin, LOW);
          digitalWrite(xstepPin, HIGH);
          delayMicroseconds(10);
          digitalWrite(xstepPin, LOW);
          delayMicroseconds(10);
          D_x_axis[D_i]++;
        }

        if (D_y_axis[D_i] < 0) {
          digitalWrite(ydirPin, LOW);
          digitalWrite(zdirPin, LOW);
          digitalWrite(ystepPin, HIGH);
          digitalWrite(zstepPin, HIGH);
          delayMicroseconds(10);
          digitalWrite(ystepPin, LOW);
          digitalWrite(zstepPin, LOW);
          delayMicroseconds(10);
          D_y_axis[D_i]++;
        }
        //Serial.println(xmoveSteps);
        //Serial.println(ymoveSteps);
        delayMicroseconds(motorDelay);
      }
      Serial1.print(D_i);
    }
    else if (D_i >= Dnum_02)
      break;
  }
  // mode_E，點位自定義模式
  while (E_Point_custom == true) {
    char keys = keypad.getKey();
    char words = Serial1.read();
    digitalWrite(G_LED, HIGH);
    digitalWrite(R_LED, LOW);
    while (E_x_axis[Enum_01] != 0 || E_y_axis[Enum_01] != 0) {
      if (E_x_axis[Enum_01] > 0) {
        digitalWrite(xdirPin, HIGH);
        digitalWrite(xstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(xstepPin, LOW);
        delayMicroseconds(10);
        E_x_axis[Enum_01]--;
      }

      if (E_y_axis[Enum_01] > 0) {
        digitalWrite(ydirPin, HIGH);
        digitalWrite(zdirPin, HIGH);
        digitalWrite(ystepPin, HIGH);
        digitalWrite(zstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(ystepPin, LOW);
        digitalWrite(zstepPin, LOW);
        delayMicroseconds(10);
        E_y_axis[Enum_01]--;
      }

      if (E_x_axis[Enum_01] < 0) {
        digitalWrite(xdirPin, LOW);
        digitalWrite(xstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(xstepPin, LOW);
        delayMicroseconds(10);
        E_x_axis[Enum_01]++;
      }

      if (E_y_axis[Enum_01] < 0) {
        digitalWrite(ydirPin, LOW);
        digitalWrite(zdirPin, LOW);
        digitalWrite(ystepPin, HIGH);
        digitalWrite(zstepPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(ystepPin, LOW);
        digitalWrite(zstepPin, LOW);
        delayMicroseconds(10);
        E_y_axis[Enum_01]++;
      }
      //Serial.println(B_x_axis[Enum_01]);
      //Serial.println(B_y_axis[Enum_01]);
      delayMicroseconds(motorDelay);
    }
    lcd.setCursor(8, 0);
    lcd.print("N:        ");
    lcd.setCursor(10, 0);
    lcd.print(Enum_01);
    Enum_01++;
    if (Enum_01 == B_i || Enum_01 > B_i) {
      digitalWrite(G_LED, LOW);
      digitalWrite(R_LED, HIGH);
      Serial1.println("Ending");
      B_i = 1;
      Enum_01 = 1;
      delaytime = 0;
      E_Point_custom = false;
      break;
    }
    digitalWrite(R_LED, HIGH);
    delay(delaytime);
    digitalWrite(R_LED, LOW);
  }
}

// 模式C、D步數換算
void mathSteps(long & moveSteps, char symbol, int num_01, int num_02, int num_03, int num_04, int rowlocation, int collocation, char axis, long xy_axis[], long xy_axis_01[], int &num) {
  // 僅在模式C下運作
  if (C_Semi_automatic == true) {
    // axis steps
    lcd.setCursor(7, collocation);
    lcd.print("       ");
    Serial1.print(symbol);
    float numDistance = (10 * num_01) + (1 * num_02) + (0.1 * num_03) + (0.01 * num_04);
    if (symbol == '-') {
      numDistance = 0 - numDistance;
    }
    Serial1.println(numDistance);
    moveSteps = static_cast<long>((static_cast<double>(numDistance) * 3200) / 0.8);
    if (moveSteps < 200 && num_04 == 5 && symbol == '+')
      moveSteps += 1;
    else if (moveSteps < 200 && num_04 == 5 && symbol == '-')
      moveSteps -= 1;
    Serial1.println(moveSteps);
    lcd.setCursor(rowlocation, collocation);
    lcd.print("                    ");
    lcd.setCursor(rowlocation, collocation);
    lcd.print(axis);
    lcd.print("Step: ");
    lcd.print(moveSteps);
  }
  // 僅在模式D下運作
  if (D_Automatic == true) {
    // 進行運算、儲存...
    float numDistance = (10 * num_01) + (1 * num_02) + (0.1 * num_03) + (0.01 * num_04);
    if (symbol == '-') {
      numDistance = 0 - numDistance;
    }
    moveSteps = static_cast<long>((static_cast<double>(numDistance) * 3200) / 0.8);
    if (moveSteps < 200 && num_04 == 5 && symbol == '+')
      moveSteps += 1;
    else if (moveSteps < 200 && num_04 == 5 && symbol == '-')
      moveSteps -= 1;
    //Serial.println(moveSteps);
    xy_axis[num] = moveSteps;
    xy_axis_01[num] = moveSteps;
  }
}

// 數字位置識別與閃爍(num: 需要變更的數字, flashingLocation: 閃爍位置, location: 數字位置)
void numLocationRecognition(int &num, int COL_flashingLocation, int ROW_flashingLocation, int location) {
  while (true) {
    // 引用副程式，num: 該位置數值、flashingLocation: 閃爍位置
    numChanges(num, COL_flashingLocation, ROW_flashingLocation);
    // 按下"s"取消數值變更後跳出
    if (numlocation != location)
      break;
    // 按下"m"後確認返回
    if (modeReturn ==  true) {
      if (D_Automatic == true)
        modeLCD_display("", 2, 1, online, "Again ", "  OK  ", "      ");
      else
        modeLCD_display("", 2, 1, online, "      ", "  OK  ", "      ");
      modeReturn = false;
      A_Correction = false;
      C_Semi_automatic = false;
      D_Automatic = false;
      E_Point_custom = false;
      Again = false;
      lcd.setCursor(8, 0);
      lcd.print("return  ");
      break;
    }
    // 在模式A中按下'o'啟動馬達，校正模式
    if (A_Correction == true && modeStart == true && D_Automatic == false) {
      modeStart = false;
      Anum_03 += Anum_02;
      lcd.setCursor(8, 0);
      lcd.print("moving");
      modeLCD_display("", 2, 1, online, "      ", "      ", "      ");
      motor_ON();
      break;
    }
    // 在模式C中按下'o'啟動馬達，motor_ON副程式
    if (C_Semi_automatic == true && modeStart == true) {
      modeStart = false;
      // 呼叫步數計算副程式
      mathSteps(xmoveSteps, xsymbol, xnum_01, xnum_02, xnum_03, xnum_04, 0, 1, 'x', D_x_axis, D_x_axis_01, Dnum_02);
      mathSteps(ymoveSteps, ysymbol, ynum_01, ynum_02, ynum_03, ynum_04, 0, 2, 'y', D_y_axis, D_y_axis_01, Dnum_02);
      modeLCD_display("", 2, 1, online, "      ", "      ", "      ");
      digitalWrite(G_LED, HIGH);
      digitalWrite(R_LED, LOW);
      lcd.setCursor(8, 0);
      lcd.print("moving");
      motor_ON();
      // 恢復模式C操作介面
      digitalWrite(G_LED, LOW);
      digitalWrite(R_LED, HIGH);
      lcd.setCursor(8, 0);
      lcd.print("Input");
      lcd.setCursor(0, 1);
      lcd.print("X:");
      lcd.print(xsymbol);
      lcd.setCursor(3, 1);
      lcd.print(xnum_01);
      lcd.print(xnum_02);
      lcd.print(".");
      lcd.print(xnum_03);
      lcd.print(xnum_04);
      lcd.print(" ");
      lcd.setCursor(9, 1);
      lcd.print("Y:");
      lcd.print(ysymbol);
      lcd.setCursor(12, 1);
      lcd.print(ynum_01);
      lcd.print(ynum_02);
      lcd.print(".");
      lcd.print(ynum_03);
      lcd.print(ynum_04);
      lcd.setCursor(17, 1);
      lcd.print("(mm");
      lcd.setCursor(0, 2);
      lcd.print("                    ");
      break;
    }
    // 在模式D中按下'o'進入數值選擇...
    if (D_Automatic == true && modeStart == true) {
      modeStart = false;
      lcd.setCursor(0, 2);
      lcd.print("X:");
      lcd.print(xsymbol);
      lcd.setCursor(3, 2);
      lcd.print(xnum_01);
      lcd.print(xnum_02);
      lcd.print(".");
      lcd.print(xnum_03);
      lcd.print(xnum_04);
      lcd.setCursor(9, 2);
      lcd.print("Y:");
      lcd.print(ysymbol);
      lcd.setCursor(12, 2);
      lcd.print(ynum_01);
      lcd.print(ynum_02);
      lcd.print(".");
      lcd.print(ynum_03);
      lcd.print(ynum_04);
      lcd.setCursor(17, 2);
      lcd.print("(mm");
      modeLCD_display("", 2, 1, online, "      ", "return", "      ");
      // 若Dnum_02小於Dnum_01則+1，並儲存參數
      if (Dnum_02 <= Dnum_01) {
        lcd.setCursor(17, 1);
        lcd.print("N:");
        lcd.print(Dnum_02);
        // 歸零數值、位置重置
        /*numlocation = 0;
          xsymbol = '+', ysymbol = '+';
          xnum_01 = 0, xnum_02 = 0, xnum_03 = 0, xnum_04 = 0;
          ynum_01 = 0, ynum_02 = 0, ynum_03 = 0, ynum_04 = 0;*/
        // 引用換算副程式、儲存數值...
        mathSteps(xmoveSteps, xsymbol, xnum_01, xnum_02, xnum_03, xnum_04, 0, 2, 'x', D_x_axis, D_x_axis_01, Dnum_02);
        mathSteps(ymoveSteps, ysymbol, ynum_01, ynum_02, ynum_03, ynum_04, 0, 3, 'y', D_y_axis, D_y_axis_01, Dnum_02);
        Dnum_02++;
      } else { /* if (Dnum_02 == Dnum_01) */
        for (int i = 1; i < Dnum_02; i++) {
          Serial1.println("--------------------");
          Serial1.print("Point ");
          Serial1.print(i);
          Serial1.print(": (");
          Serial1.print(D_x_axis[i]);
          Serial1.print(", ");
          Serial1.print(D_y_axis[i]);
          Serial1.println(")");
        }
        D_x_axis[0] = B_x_axis[1];
        D_y_axis[0] = B_y_axis[1];
        Dnum_03 = Dnum_02;
        // 跳往馬達通用副程式...
        A_Correction = false;
        motor_ON();
        lcd.setCursor(0, 0);
        lcd.print("mode_D: finish  ");
        lcd.setCursor(0, 2);
        //lcd.print("press return...     ");
        modeLCD_display("", 2, 1, online, "Again ", "return", "      ");
        Dnum_01 = 0;
        Dnum_02 = 0;
        // 禁止進入'<' or '>'
        D_Automatic = false;
        Again = true;
        break;
      }
      // 這邊跳往數值選擇
      A_Correction = false;
      D_numChoose();
      break;
    }
    // 在模式E按下'o'啟動馬達
    if (E_Point_custom == true && modeStart == true) {
      modeStart = false;
      delaytime = ((Enum_02 * 10) + Enum_03) * 1000;
      // 記錄延遲時間
      Enum_05 = delaytime;
      // 記錄存儲次數
      Enum_04 = B_i;
      // 位置運算
      for (int i = 1; i <= B_i; i++) {
        E_x_axis[i] = B_x_axis[i] - B_x_axis[i - 1];
        E_y_axis[i] = B_y_axis[i] - B_y_axis[i - 1];
        // 存儲移動距離
        Serial1.println("--------------------");
        Serial1.print("Point ");
        Serial1.print(i);
        Serial1.print(": (");
        Serial1.print(B_x_axis[i]);
        Serial1.print(", ");
        Serial1.print(B_y_axis[i]);
        Serial1.println(")");
      }
      /*lcd.setCursor(0, 3);
        lcd.print("finish point: ");
        lcd.print(Enum_01);*/
      modeLCD_display("", 2, 1, online, "      ", "      ", "      ");
      motor_ON();
      modeLCD_display("", 2, 1, online, "Again ", "return", "online");
      Again = true;
      break;
    }
    // 預設數值閃爍
    lcd.setCursor(COL_flashingLocation, ROW_flashingLocation);
    lcd.print(" ");
    delay(150);
    lcd.setCursor(COL_flashingLocation, ROW_flashingLocation);
    lcd.print(num);
    delay(150);
  }
}

// 手動模式(按鍵)數值選擇
void numChanges(int &num, int COL_location, int ROW_location) {
  char keys = keypad.getKey();
  if (Serial.available() > 0)
    words = Serial.read();
  else if (Serial1.available() > 0)
    words = Serial1.read();
  // 切換線上線下模式
  if ((keys == 'p' && online == true) || words == 'P') {
    lcd.setCursor(19, 0);
    lcd.print("L");
    online = false;
    Serial1.println("# 已開啟線下模式!");
  } else if ((keys == 'p' && online == false) || words == 'O') {
    lcd.setCursor(19, 0);
    lcd.print("H");
    online = true;
    Serial1.println("# 已開啟線上模式!");
  }
  // 僅在線下模式座使用
  if (keys == '>' && online == false) {
    // 僅在模式A作用
    if (A_Correction == true)
      numlocation = 0;
    else
      numlocation += 1;
  } else if (keys == '<' && online == false) {
    // 僅在模式A作用
    if (A_Correction == true)
      numlocation = 0;
    // 僅在模式E作用
    else if (E_Point_custom == true) {
      if (numlocation > 0)
        numlocation -= 1;
      else
        numlocation = 1;
    }
    else if (numlocation > 0)
      numlocation -= 1;
    else
      numlocation = 9;
  } else if (keys == 's' && online == false) {
    while (true) {
      char keys = keypad.getKey();
      if (keys == '>') {
        lcd.setCursor(COL_location, ROW_location);
        if (num > 0)
          num -= 1;
        else
          num = 9;
        lcd.print(num);
        delay(300);
      } else if (keys == '<') {
        lcd.setCursor(COL_location, ROW_location);
        if (num < 9)
          num += 1;
        else
          num = 0;
        lcd.print(num);
        delay(300);
      } else if (keys == 's') {
        break;
      }
    }
  } else if (keys == 'm' || words == 'R') {
    Serial.println("# 已退出模式A!");
    Serial1.println("# 已退出模式A!");
    modeReturn = true;
  } else if (keys == 'o' || words == 'O') {
    Serial.println("- 馬達正在校正中...");
    Serial1.println("- 馬達正在校正中...");
    modeStart = true;
  }
  // 僅在模式A線上模式使用
  else if (A_Correction == true && online == true) {
    // 读取窗口资料
    String inputString;
    if (Serial.available() > 0)
      inputString = Serial.readStringUntil('\n');
    else if (Serial1.available() > 0)
      inputString = Serial1.readStringUntil('\n');
    int number001 = inputString.toInt();
    Anum_02 = number001;
    Dnum_01 = number001;
    lcd.setCursor(15, 1);
    lcd.print(number001);
    if (number001 != 0) {
      Serial.println(number001);
      Serial1.println(number001);
      online = !online;
      Serial.println("- 請輸入大寫字母\'O(啟動)\' or \'R(返回)\'以繼續!");
      Serial1.println("- 請輸入大寫字母\'O(啟動)\' or \'R(返回)\'以繼續!");
      number001 = 0;
    }
  }
  // 僅在模式C、D線上模式使用
  else if ((C_Semi_automatic == true && online == true) || (D_Automatic == true && online == true)) {
    Serial1.println("Input num");
    String inputString = Serial1.readStringUntil('\n');

    int commaIndex = inputString.indexOf(',');
    int spaceIndex = inputString.indexOf(' ');

    int separatorIndex = (commaIndex != -1 && (spaceIndex == -1 || commaIndex < spaceIndex)) ? commaIndex : spaceIndex;

    String number1String = inputString.substring(0, separatorIndex);
    String number2String = inputString.substring(separatorIndex + 1);

    float number002 = number1String.toFloat();
    float number003 = number2String.toFloat();
    if (number002 != 0 || number003 != 0) {
      Serial1.println(number002);
      int sign1 = (number002 < 0) ? -1 : 1;
      xsymbol = (sign1 == -1) ? '-' : '+';

      number002 = abs(number002);

      xnum_01 = int(number002) / 10;
      xnum_02 = int(number002) % 10;
      xnum_03 = int(number002 * 10) % 10;
      xnum_04 = int(number002 * 100) % 10;

      // 判別線上數值輸入為模式A、D
      if (C_Semi_automatic == true) {
        lcd.setCursor(2, 1);
        lcd.print(xsymbol);
        lcd.print(xnum_01);
        lcd.print(xnum_02);
        lcd.setCursor(6, 1);
        lcd.print(xnum_03);
        lcd.print(xnum_04);

        /*Serial.println(xsymbol);
          Serial.println(xnum_01);
          Serial.println(xnum_02);
          Serial.println(xnum_03);
          Serial.println(xnum_04);*/
      } else if (D_Automatic == true) {
        lcd.setCursor(2, 2);
        lcd.print(xsymbol);
        lcd.print(xnum_01);
        lcd.print(xnum_02);
        lcd.setCursor(6, 2);
        lcd.print(xnum_03);
        lcd.print(xnum_04);
      }

      Serial1.println(number003);
      int sign2 = (number003 < 0) ? -1 : 1;
      ysymbol = (sign2 == -1) ? '-' : '+';

      number003 = abs(number003);

      ynum_01 = int(number003) / 10;
      ynum_02 = int(number003) % 10;
      ynum_03 = int(number003 * 10) % 10;
      ynum_04 = int(number003 * 100) % 10;

      if (C_Semi_automatic == true) {
        lcd.setCursor(11, 1);
        lcd.print(ysymbol);
        lcd.print(ynum_01);
        lcd.print(ynum_02);
        lcd.setCursor(15, 1);
        lcd.print(ynum_03);
        lcd.print(ynum_04);
        /*
          Serial.println(ysymbol);
          Serial.println(ynum_01);
          Serial.println(ynum_02);
          Serial.println(ynum_03);
          Serial.println(ynum_04);*/
      } else if (D_Automatic == true) {
        lcd.setCursor(11, 2);
        lcd.print(ysymbol);
        lcd.print(ynum_01);
        lcd.print(ynum_02);
        lcd.setCursor(15, 2);
        lcd.print(ynum_03);
        lcd.print(ynum_04);
      }

      online = !online;
    }
  }
  return;
}

// 手動模式(按鍵)符號選擇
void symbolChanges(char &symbol, int COL_location, int ROW_location) {
  char keys = keypad.getKey();
  Serial1.print(keys);
  if (keys == '>') {
    numlocation += 1;
  } else if (keys == '<') {
    if (numlocation > 0)
      numlocation -= 1;
    else
      numlocation = 9;
  } else if (keys == 's') {
    while (true) {
      char keys = keypad.getKey();
      if (keys == '>') {
        lcd.setCursor(COL_location, ROW_location);
        if (symbol == '+')
          symbol = '-';
        else if (symbol == '-')
          symbol = '+';
        lcd.print(symbol);
        delay(300);
      } else if (keys == '<') {
        lcd.setCursor(COL_location, ROW_location);
        if (symbol == '-')
          symbol = '+';
        else if (symbol == '+')
          symbol = '-';
        lcd.print(symbol);
        delay(300);
      } else if (keys == 's') {
        break;
      }
    }
  } else if (keys == 'm') {
    modeReturn = true;
  } else if (keys == 'o') {
    modeStart = true;
  }
  return;
}

// 模式顯示
void modeLCD_display(String mode, int ROWS_location, int COLS_location, bool online, String display_01, String display_02, String display_03) {
  // state display
  lcd.setCursor(0, 3);
  lcd.print(display_01);
  lcd.setCursor(6, 3);
  lcd.print("|");
  lcd.setCursor(7, 3);
  lcd.print(display_02);
  lcd.setCursor(13, 3);
  lcd.print("|");
  lcd.setCursor(14, 3);
  lcd.print(display_03);
  // online display
  if (online == true) {
    lcd.setCursor(17, 0);
    lcd.print("O:");
    lcd.setCursor(19, 0);
    lcd.print("H");
  } else {
    lcd.setCursor(17, 0);
    lcd.print("O:");
    lcd.setCursor(19, 0);
    lcd.print("L");
  }
  // mode display
  if (mode == "mode_A") {
    lcd.setCursor(0, 0);
    lcd.print("Welcome using,");
    lcd.setCursor(2, 2);
    lcd.print("mode_B");
    lcd.setCursor(12, 1);
    lcd.print("mode_C");
    lcd.setCursor(12, 2);
    lcd.print("mode_D");
    selectedMode("mode_A", 2, 1);
  } else if (mode == "mode_B") {
    lcd.setCursor(0, 0);
    lcd.print("Welcome using,");
    lcd.setCursor(2, 1);
    lcd.print("mode_A");
    lcd.setCursor(12, 1);
    lcd.print("mode_C");
    lcd.setCursor(12, 2);
    lcd.print("mode_D");
    selectedMode("mode_B", 2, 2);
  } else if (mode == "mode_C") {
    lcd.setCursor(0, 0);
    lcd.print("Welcome using,");
    lcd.setCursor(2, 1);
    lcd.print("mode_A");
    lcd.setCursor(2, 2);
    lcd.print("mode_B");
    lcd.setCursor(12, 2);
    lcd.print("mode_D");
    selectedMode("mode_C", 12, 1);
  } else if (mode == "mode_D") {
    lcd.setCursor(0, 0);
    lcd.print("Welcome using,");
    lcd.setCursor(2, 1);
    lcd.print("mode_A");
    lcd.setCursor(2, 2);
    lcd.print("mode_B");
    lcd.setCursor(12, 1);
    lcd.print("mode_C");
    selectedMode("mode_D", 12, 2);
  } else if (mode == "mode_E") {
    lcd.setCursor(0, 0);
    lcd.print("Welcome using,");
    lcd.setCursor(2, 2);
    lcd.print("mode_T");
    lcd.setCursor(12, 1);
    lcd.print("mode_V");
    selectedMode("mode_E", 2, 1);
  } else if (mode == "mode_T") {
    lcd.setCursor(0, 0);
    lcd.print("Welcome using,");
    lcd.setCursor(2, 1);
    lcd.print("mode_E");
    lcd.setCursor(12, 1);
    lcd.print("mode_V");
    selectedMode("mode_T", 2, 2);
  } else if (mode == "mode_V") {
    lcd.setCursor(0, 0);
    lcd.print("Welcome using,");
    lcd.setCursor(2, 1);
    lcd.print("mode_E");
    lcd.setCursor(2, 2);
    lcd.print("mode_T");
    selectedMode("mode_V", 12, 1);
  }
}

// 模式切換
void selectedMode(String mode, int ROWS_location, int COLS_location) {
  if (mode == "mode_A" || mode == "mode_E") {
    lcd.setCursor(ROWS_location - 2, COLS_location);
    lcd.print(">");
    lcd.setCursor(ROWS_location + 7, COLS_location);
    lcd.print("<");
  } else if (mode == "mode_B" || mode == "mode_D" || mode == "mode_T") {
    lcd.setCursor(ROWS_location - 2, COLS_location - 1);
    lcd.print(" ");
    lcd.setCursor(ROWS_location + 7, COLS_location - 1);
    lcd.print(" ");
    lcd.setCursor(ROWS_location - 2, COLS_location);
    lcd.print(">");
    lcd.setCursor(ROWS_location + 7, COLS_location);
    lcd.print("<");
  } else if (mode == "mode_C" || mode == "mode_V") {
    lcd.setCursor(ROWS_location - 12, COLS_location + 1);
    lcd.print(" ");
    lcd.setCursor(ROWS_location - 3, COLS_location + 1);
    lcd.print(" ");
    lcd.setCursor(ROWS_location - 2, COLS_location);
    lcd.print(">");
    lcd.setCursor(ROWS_location + 7, COLS_location);
    lcd.print("<");
  }
  lcd.setCursor(ROWS_location, COLS_location);
  lcd.print("      ");
  delay(150);
  lcd.setCursor(ROWS_location, COLS_location);
  lcd.print(mode);
  delay(300);
}

// 數值選擇，僅在模式D下使用
void D_numChoose() {
  while (true) {
    char keys = keypad.getKey();
    char words = Serial1.read();
    // 切換線上線下模式
    if ((keys == 'p' && online == true) || words == 'O') {
      Serial1.println("# 已進入線下模式!");
      online = false;
    } else if ((keys == 'p' && online == false) || words == 'P') {
      Serial1.println("# 已進入線上模式!");
      keys = '<';
      online = true;
    } else if (words == 'R') {
      Serial1.println("# press A~D choose mode!");
      break;
    }
    switch (keys) {
      case '<':
        while (D_Automatic == true) {
          modeLCD_display("", 2, 1, online, "  OK  ", "return", "      ");
          if (numlocation == 9) {
            while (true) {
              symbolChanges(xsymbol, 2, 2);
              if (numlocation != 9)
                break;
              if (modeReturn == true) {
                modeReturn = false;
                C_Semi_automatic = false;
                lcd.setCursor(8, 0);
                lcd.print("return  ");
                break;
              }
              // 這個須修改
              if (modeStart == true) {
                modeStart = false;
                // 暫無用處
                //mathSteps(xmoveSteps, xsymbol, xnum_01, xnum_02, xnum_03, xnum_04, 0, 2, 'x');
                //mathSteps(ymoveSteps, ysymbol, ynum_01, ynum_02, ynum_03, ynum_04, 0, 3, 'y');
                //motor_ON();
                break;
              }
              lcd.setCursor(2, 2);
              lcd.print(" ");
              delay(150);
              lcd.setCursor(0, 2);
              lcd.print("X:");
              lcd.print(xsymbol);
              delay(150);
            }
          } else if (numlocation == 8) {
            numLocationRecognition(xnum_01, 3, 2, 8);
          } else if (numlocation == 7) {
            numLocationRecognition(xnum_02, 4, 2, 7);
          } else if (numlocation == 6) {
            numLocationRecognition(xnum_03, 6, 2, 6);
          } else if (numlocation == 5) {
            numLocationRecognition(xnum_04, 7, 2, 5);
          }// ysymbol
          else if (numlocation == 4) {
            while (true) {
              symbolChanges(ysymbol, 11, 2);
              if (numlocation != 4)
                break;
              if (modeReturn == true) {
                modeReturn = false;
                C_Semi_automatic = false;
                lcd.setCursor(8, 0);
                lcd.print("return  ");
                break;
              }
              if (modeStart == true) {
                modeStart = false;
                //mathSteps(xmoveSteps, xsymbol, xnum_01, xnum_02, xnum_03, xnum_04, 0, 2, 'x');
                //mathSteps(ymoveSteps, ysymbol, ynum_01, ynum_02, ynum_03, ynum_04, 0, 3, 'y');
                //motor_ON();
                break;
              }
              lcd.setCursor(11, 2);
              lcd.print(" ");
              delay(150);
              lcd.setCursor(9, 2);
              lcd.print("Y:");
              lcd.print(ysymbol);
              delay(150);
            }
          } else if (numlocation == 3) {
            numLocationRecognition(ynum_01, 12, 2, 3);
          } else if (numlocation == 2) {
            numLocationRecognition(ynum_02, 13, 2, 2);
          } else if (numlocation == 1) {
            numLocationRecognition(ynum_03, 15, 2, 1);
          } else if (numlocation == 0) {
            numLocationRecognition(ynum_04, 16, 2, 0);
          }
          // reset
          else {
            numlocation = 0;
          }
        }
        break;

      case '>':
        while (D_Automatic == true) {
          modeLCD_display("", 2, 1, online, "  OK  ", "return", "      ");
          if (numlocation == 9) {
            while (true) {
              symbolChanges(xsymbol, 2, 2);
              if (numlocation != 9)
                break;
              if (modeReturn == true) {
                modeReturn = false;
                C_Semi_automatic = false;
                lcd.setCursor(8, 0);
                lcd.print("return  ");
                break;
              }
              if (modeStart == true) {
                modeStart = false;
                //mathSteps(xmoveSteps, xsymbol, xnum_01, xnum_02, xnum_03, xnum_04, 0, 2, 'x');
                //mathSteps(ymoveSteps, ysymbol, ynum_01, ynum_02, ynum_03, ynum_04, 0, 3, 'y');
                //motor_ON();
                break;
              }
              lcd.setCursor(2, 2);
              lcd.print(" ");
              delay(150);
              lcd.setCursor(0, 2);
              lcd.print("X:");
              lcd.print(xsymbol);
              delay(150);
            }
          } else if (numlocation == 8) {
            numLocationRecognition(xnum_01, 3, 2, 8);
          } else if (numlocation == 7) {
            numLocationRecognition(xnum_02, 4, 2, 7);
          } else if (numlocation == 6) {
            numLocationRecognition(xnum_03, 6, 2, 6);
          } else if (numlocation == 5) {
            numLocationRecognition(xnum_04, 7, 2, 5);
          }// ysymbol
          else if (numlocation == 4) {
            while (true) {
              symbolChanges(ysymbol, 11, 2);
              if (numlocation != 4)
                break;
              if (modeReturn == true) {
                modeReturn = false;
                C_Semi_automatic = false;
                lcd.setCursor(8, 0);
                lcd.print("return  ");
                break;
              }
              if (modeStart == true) {
                modeStart = false;
                //mathSteps(xmoveSteps, xsymbol, xnum_01, xnum_02, xnum_03, xnum_04, 0, 2, 'x');
                //mathSteps(ymoveSteps, ysymbol, ynum_01, ynum_02, ynum_03, ynum_04, 0, 3, 'y');
                //motor_ON();
                break;
              }
              lcd.setCursor(11, 2);
              lcd.print(" ");
              delay(150);
              lcd.setCursor(9, 2);
              lcd.print("Y:");
              lcd.print(ysymbol);
              delay(150);
            }
          } else if (numlocation == 3) {
            numLocationRecognition(ynum_01, 12, 2, 3);
          } else if (numlocation == 2) {
            numLocationRecognition(ynum_02, 13, 2, 2);
          } else if (numlocation == 1) {
            numLocationRecognition(ynum_03, 15, 2, 1);
          } else if (numlocation == 0) {
            numLocationRecognition(ynum_04, 16, 2, 0);
          }
          // reset
          else {
            numlocation = 0;
          }
        }
        break;
      // 按下's'直接跳出
      case 's':
        modeReturn = true;
        break;
      // 模式切換、返回
      case 'm':
        if (modeChange < 6) {
          modeChange += 1;
          lcd.clear();
          if (modeChange == 1) {
            modeLCD_display("mode_B", 2, 2, online, "      ", " mode ", "online");
          } else if (modeChange == 2) {
            modeLCD_display("mode_C", 12, 1, online, "      ", " mode ", "online");
          } else if (modeChange == 3) {
            modeLCD_display("mode_D", 12, 2, online, "      ", " mode ", "online");
          } else if (modeChange == 4) {
            lcd.clear();
            modeLCD_display("mode_E", 2, 1, online, "      ", " mode ", "online");
          } else if (modeChange == 5) {
            modeLCD_display("mode_T", 2, 2, online, "      ", " mode ", "online");
          } else if (modeChange == 6) {
            modeLCD_display("mode_V", 12, 1, online, "      ", " mode ", "online");
          }
        } else {
          modeChange = 0;
          lcd.clear();
          modeLCD_display("mode_A", 2, 1, online, "      ", " mode ", "online");
        }
        break;
      case 'o':
        if (Again == true) {
          Again = false;
          // fix
          for (int i = 1; i < Dnum_03; i++) {
            Serial1.println("--------------------");
            /* Serial.print("Point ");
              Serial.print(i);
              Serial.print(": (");
              Serial.print(D_x_axis_01[i]);
              Serial.print(", ");
              Serial.print(D_y_axis_01[i]);
              Serial.println(")");*/
            D_x_axis[i] = D_x_axis_01[i];
            D_y_axis[i] = D_y_axis_01[i];
            Serial1.print("Point ");
            Serial1.print(i);
            Serial1.print(": (");
            Serial1.print(D_x_axis[i]);
            Serial1.print(", ");
            Serial1.print(D_y_axis[i]);
            Serial1.println(")");
          }
          D_x_axis[0] = B_x_axis[1];
          D_y_axis[0] = B_y_axis[1];
          Dnum_02 = Dnum_03;
          D_i = 0;
          D_Automatic = true;
          motor_ON();
          break;
        }
    }
    if (modeReturn == true) {
      modeReturn = false;
      lcd.setCursor(0, 0);
      lcd.print("mode: press 's' ");
      break;
    }
  }
}

// 模式(按鍵)
void mode_Offline(int keys) {
  switch (keys) {
    // 手動模式(數值)下數值減少，左(選到數字時後閃爍)
    case '<':
      // 僅在模式A下使用
      while (A_Correction == true) {
        // Anum_02
        modeLCD_display("", 2, 1, online, "enbale", "return", "online");
        numLocationRecognition(Anum_02, 15, 1, 0);
        break;
      }
      // 僅在模式C下使用
      while (C_Semi_automatic == true) {
        modeLCD_display("", 2, 1, online, "enbale", "return", "online");
        // xsymbol
        if (numlocation == 9) {
          while (true) {
            symbolChanges(xsymbol, 2, 1);
            if (numlocation != 9)
              break;
            if (modeReturn == true) {
              modeReturn = false;
              C_Semi_automatic = false;
              lcd.setCursor(8, 0);
              modeLCD_display("", 2, 1, online, "      ", "  OK  ", "      ");
              lcd.print("return");
              break;
            }
            if (modeStart == true) {
              modeStart = false;
              mathSteps(xmoveSteps, xsymbol, xnum_01, xnum_02, xnum_03, xnum_04, 0, 1, 'x', D_x_axis, D_x_axis_01, Dnum_02);
              mathSteps(ymoveSteps, ysymbol, ynum_01, ynum_02, ynum_03, ynum_04, 0, 2, 'y', D_y_axis, D_y_axis_01, Dnum_02);
              motor_ON();
              break;
            }
            lcd.setCursor(2, 1);
            lcd.print(" ");
            delay(150);
            lcd.setCursor(0, 1);
            lcd.print("X:");
            lcd.print(xsymbol);
            delay(150);
          }
        }
        // xnum_01
        else if (numlocation == 8) {
          numLocationRecognition(xnum_01, 3, 1, 8);
        }
        // xnum_02
        else if (numlocation == 7) {
          numLocationRecognition(xnum_02, 4, 1, 7);
        }
        // xnum_03
        else if (numlocation == 6) {
          numLocationRecognition(xnum_03, 6, 1, 6);
        }
        // xnum_04
        else if (numlocation == 5) {
          numLocationRecognition(xnum_04, 7, 1, 5);
        }
        // ysymbol
        else if (numlocation == 4) {
          while (true) {
            symbolChanges(ysymbol, 11, 1);
            if (numlocation != 4)
              break;
            if (modeReturn == true) {
              modeReturn = false;
              C_Semi_automatic = false;
              lcd.setCursor(8, 0);
              lcd.print("return");
              modeLCD_display("", 2, 1, online, "      ", "  OK  ", "      ");
              break;
            }
            if (modeStart == true) {
              modeStart = false;
              mathSteps(xmoveSteps, xsymbol, xnum_01, xnum_02, xnum_03, xnum_04, 0, 1, 'x', D_x_axis, D_x_axis_01, Dnum_02);
              mathSteps(ymoveSteps, ysymbol, ynum_01, ynum_02, ynum_03, ynum_04, 0, 2, 'y', D_y_axis, D_y_axis_01, Dnum_02);
              motor_ON();
              break;
            }
            lcd.setCursor(11, 1);
            lcd.print(" ");
            delay(150);
            lcd.setCursor(9, 1);
            lcd.print("Y:");
            lcd.print(ysymbol);
            delay(150);
          }
        }
        // ynum_01
        else if (numlocation == 3) {
          numLocationRecognition(ynum_01, 12, 1, 3);
        }
        // ynum_02
        else if (numlocation == 2) {
          numLocationRecognition(ynum_02, 13, 1, 2);
        }
        // ynum_03
        else if (numlocation == 1) {
          numLocationRecognition(ynum_03, 15, 1, 1);
        }
        // ynum_04
        else if (numlocation == 0) {
          numLocationRecognition(ynum_04, 16, 1, 0);
        }
        // reset
        else {
          numlocation = 0;
        }
      }
      // 僅在模式D下使用
      while (D_Automatic == true) {
        modeLCD_display("", 2, 1, online, " next ", "return", "online");
        A_Correction = true;
        numLocationRecognition(Dnum_01, 15, 1, 0);
        //A_Correction = false;
        break;
      }
      // 僅在模式E下使用
      while (E_Point_custom == true) {
        modeLCD_display("", 2, 1, online, "enbale", "return", "      ");
        if (numlocation == 1)
          numLocationRecognition(Enum_02, 16, 2, 1);
        else if (numlocation == 0)
          numLocationRecognition(Enum_03, 17, 2, 0);
        else
          numlocation = 0;
      }
      break;

    // 手動模式(數值)下數值增加，右(選到數字時後閃爍)
    case '>':
      // 僅在模式A下使用
      while (A_Correction == true) {
        // Anum_01
        modeLCD_display("", 2, 1, online, "enbale", "return", "online");
        numLocationRecognition(Anum_02, 15, 1, 0);
      }
      // 僅在模式C下使用
      while (C_Semi_automatic == true) {
        modeLCD_display("", 2, 1, online, "enbale", "return", "online");
        // xsymbol
        if (numlocation == 9) {
          while (true) {
            symbolChanges(xsymbol, 2, 1);
            if (numlocation != 9)
              break;
            if (modeReturn == true) {
              modeReturn = false;
              C_Semi_automatic = false;
              lcd.setCursor(8, 0);
              lcd.print("return");
              modeLCD_display("", 2, 1, online, "      ", "  OK  ", "      ");
              break;
            }
            if (modeStart == true) {
              modeStart = false;
              D_modeStart = false;
              motor_ON();
              break;
            }
            lcd.setCursor(2, 1);
            lcd.print(" ");
            delay(150);
            lcd.setCursor(0, 1);
            lcd.print("X:");
            lcd.print(xsymbol);
            delay(150);
          }
        }
        // xnum_01
        else if (numlocation == 8) {
          numLocationRecognition(xnum_01, 3, 1, 8);
        }
        // xnum_02
        else if (numlocation == 7) {
          numLocationRecognition(xnum_02, 4, 1, 7);
        }
        // xnum_03
        else if (numlocation == 6) {
          numLocationRecognition(xnum_03, 6, 1, 6);
        }
        // xnum_04
        else if (numlocation == 5) {
          numLocationRecognition(xnum_04, 7, 1, 5);
        }
        // ysymbol
        else if (numlocation == 4) {
          while (true) {
            symbolChanges(ysymbol, 11, 1);
            if (numlocation != 4)
              break;
            if (modeReturn == true) {
              modeReturn = false;
              C_Semi_automatic = false;
              lcd.setCursor(8, 0);
              lcd.print("return");
              modeLCD_display("", 2, 1, online, "      ", "  OK  ", "      ");
              break;
            }
            if (modeStart == true) {
              modeStart = false;
              motor_ON();
              break;
            }
            lcd.setCursor(11, 1);
            lcd.print(" ");
            delay(150);
            lcd.setCursor(9, 1);
            lcd.print("Y:");
            lcd.print(ysymbol);
            delay(150);
          }
        }
        // ynum_01
        else if (numlocation == 3) {
          numLocationRecognition(ynum_01, 12, 1, 3);
        }
        // ynum_02
        else if (numlocation == 2) {
          numLocationRecognition(ynum_02, 13, 1, 2);
        }
        // ynum_03
        else if (numlocation == 1) {
          numLocationRecognition(ynum_03, 15, 1, 1);
        }
        // ynum_04
        else if (numlocation == 0) {
          numLocationRecognition(ynum_04, 16, 1, 0);
        }
        // reset
        else {
          numlocation = 0;
        }
      }
      // 僅在模式D下使用
      while (D_Automatic == true) {
        A_Correction = true;
        numLocationRecognition(Dnum_01, 15, 1, 0);
        //A_Correction = false;
        break;
      }
      // 僅在模式E下使用
      while (E_Point_custom == true) {
        modeLCD_display("", 2, 1, online, "enbale", "return", "      ");
        if (numlocation == 1)
          numLocationRecognition(Enum_02, 16, 2, 1);
        else if (numlocation == 0)
          numLocationRecognition(Enum_03, 17, 2, 0);
        else
          numlocation = 0;
      }
      break;

    // 數值選擇、模式確認
    case 's':
      switch (modeChange) {
        case 0:
          mode_A();
          B_Manual = false;
          C_Semi_automatic = false;
          D_Automatic = false;
          E_Point_custom = false;
          break;
        case 1:
          xsymbol = '.', ysymbol = '.';
          online = false;
          A_Correction = false;
          C_Semi_automatic = false;
          D_Automatic = false;
          E_Point_custom = false;
          mode_B();
          break;
        case 2:
          // 數值歸零
          xsymbol = '+', ysymbol = '+';
          xnum_01 = 0, xnum_02 = 0, xnum_03 = 0, xnum_04 = 0;
          ynum_01 = 0, ynum_02 = 0, ynum_03 = 0, ynum_04 = 0;
          mode_C();
          A_Correction = false;
          B_Manual = false;
          D_Automatic = false;
          E_Point_custom = false;
          break;
        case 3:
          // 數值歸零
          xsymbol = '+', ysymbol = '+';
          xnum_01 = 0, xnum_02 = 0, xnum_03 = 0, xnum_04 = 0;
          ynum_01 = 0, ynum_02 = 0, ynum_03 = 0, ynum_04 = 0;
          mode_D();
          A_Correction = false;
          B_Manual = false;
          C_Semi_automatic = false;
          E_Point_custom = false;
          break;
        case 4:
          mode_E();
          A_Correction = false;
          B_Manual = false;
          C_Semi_automatic = false;
          D_Automatic = false;
          break;
        case 5:
          mode_T();
          break;
        case 6:
          mode_V();
          break;
      }
      break;

    // 模式切換、返回
    case 'm':
      if (modeChange < 6) {
        // 模式E出來後關閉Again
        Again = false;
        modeChange += 1;
        lcd.clear();
        if (modeChange == 1) {
          modeLCD_display("mode_B", 2, 2, online, "      ", " mode ", "online");
        } else if (modeChange == 2) {
          modeLCD_display("mode_C", 12, 1, online, "      ", " mode ", "online");
        } else if (modeChange == 3) {
          modeLCD_display("mode_D", 12, 2, online, "      ", " mode ", "online");
        } else if (modeChange == 4) {
          lcd.clear();
          modeLCD_display("mode_E", 2, 1, online, "      ", " mode ", "online");
        } else if (modeChange == 5) {
          modeLCD_display("mode_T", 2, 2, online, "      ", " mode ", "online");
        } else if (modeChange == 6) {
          modeLCD_display("mode_V", 12, 1, online, "      ", " mode ", "online");
        }
      } else {
        modeChange = 0;
        lcd.clear();
        modeLCD_display("mode_A", 2, 1, online, "      ", " mode ", "online");
      }
      break;

    // 線上/線下模式
    case 'p':
      // 切換線上線下模式
      if (keys == 'p' && online == true) {
        Serial1.println("# 已進入線下模式!");
        lcd.setCursor(19, 0);
        lcd.print("L");
        online = false;
      } else if (keys == 'p' && online == false) {
        Serial.println("# 已進入線上模式!");
        lcd.setCursor(19, 0);
        lcd.print("H");
        online = true;
      }
      break;

    case 'o':
      // 僅在模式E時使用
      if (Again == true) {
        E_Point_custom = true;
        // 預先歸零
        digitalWrite(xdirPin, LOW);
        digitalWrite(ydirPin, LOW);
        digitalWrite(zdirPin, LOW);
        Correction_procedure(2);
        for (int i = 1; i < Enum_04; i++) {
          E_x_axis[i] = B_x_axis[i];
          E_y_axis[i] = B_y_axis[i];
          Serial1.print("Point ");
          Serial1.print(i);
          Serial1.print(": (");
          Serial1.print(B_x_axis[i]);
          Serial1.print(", ");
          Serial1.print(B_y_axis[i]);
          Serial1.println(")");
        }
        delaytime = Enum_05;
        B_i = Enum_04;
        Enum_01 = 0;
        motor_ON();
        break;
      }
  }
}

// 校正模式(常規(紅)、碰到限位開關(藍)、移動(綠))
void mode_A() {
  Serial.println("# 當前已進入模式A，點位校正(Correction Mode)模式!");
  Serial.print("- 請輸入校正次數(建議02~09次):");
  Serial1.println("# 當前已進入模式A，點位校正(Correction Mode)模式!");
  Serial1.print("- 請輸入校正次數(建議02~09次):");
  Anum_02 = 0;
  lcd.clear();
  LCD_Display(0, 0, "mode_A: Input ");
  LCD_Display(0, 1, "set frequency: ");
  lcd.print(Anum_02);
  modeLCD_display("", 2, 1, online, "      ", "return", "      ");
  // 模式A_校正模式啟用
  A_Correction = true;
}

// 手動模式(常規(紅)、碰到限位開關(藍)、移動(綠))
void mode_B() {
  // Serial Display
  Serial.println("# 當前已進入模式B，搖桿調整(Manual Mode)模式!");
  Serial1.println("# 當前已進入模式B，搖桿調整(Manual Mode)模式!");
  // 參數歸零
  x_coordinate = 0;
  y_coordinate = 0;
  B_i = 1;
  // LCD Display
  lcd.clear();
  LCD_Display(0, 0, "mode_B: moving");
  modeLCD_display("", 2, 1, online, "      ", "      ", "      ");
  // 预先校正
  digitalWrite(xdirPin, LOW);
  digitalWrite(ydirPin, LOW);
  digitalWrite(zdirPin, LOW);
  Correction_procedure(2);
  Serial.println("- 請輸入數字'O(紀錄)'、'1~4, 6~9(方向)'、'R(返回)'、5'(停止)'以繼續!");
  Serial1.println("- 請輸入數字'O(紀錄)'、'1~4, 6~9(方向)'、'R(返回)'、'5(停止)'以繼續!");
  LCD_Display(0, 0, "mode_B: Input ");
  LCD_Display(3, 1, "press Joystick");
  modeLCD_display("", 2, 1, online, "      ", "return", "online");
  while (true) {
    int buttonState = digitalRead(joyButton);
    char keys = keypad.getKey();
    if (Serial.available() > 0)
      words = Serial.read();
    else if (Serial1.available() > 0)
      words = Serial1.read();
    if (modeReturn == true || keys == 'm' || words == 'R') {
      lcd.clear();
      lcd.setCursor(3, 3);
      lcd.setCursor(0, 0);
      lcd.print("mode_B: return");
      modeLCD_display("", 2, 1, online, "      ", "  OK  ", "      ");
      Serial1.println("press A~D Choose mode!");
      modeReturn = false;
      buttonState = 0;
      button = 0;
      break;
    }/*
    if ((keys == 'p' && online == true) || words == 'P') {
      lcd.setCursor(19, 0);
      lcd.print("L");
      online = false;
      Serial1.println("# 已開啟線下模式!");
    } else if ((keys == 'p' && online == false) || words == 'O') {
      lcd.setCursor(19, 0);
      lcd.print("H");
      online = true;
      Serial1.println("# 已開啟線上模式!");
    }*/
    while (buttonState == 0 || online == true) {
      char keys = keypad.getKey();
      if (Serial.available() > 0)
        words = Serial.read();
      else if (Serial1.available() > 0)
        words = Serial1.read();
      digitalWrite(G_LED, HIGH);
      digitalWrite(R_LED, HIGH);
      int xValue = analogRead(joyX);
      int yValue = analogRead(joyY);
      int buttonState = digitalRead(joyButton);
      //Serial.print("\tButton: ");
      //Serial.println(buttonState);
      modeLCD_display("", 2, 1, online, "record", "return", "      ");
      // 状态面板显示 (通過按鍵's'切換 for 10/31)
      if (B_display == false) {
        LCD_Display(0, 1, "                   ");
        LCD_Display(0, 2, "                   ");
        LCD_Display(3, 1, "    Stick ON    ");
        LCD_Display(3, 2, "X: ");
        lcd.print(xValue);
        LCD_Display(11, 2, "Y: ");
        lcd.print(yValue);
        delay(250);
      } else if (B_display == true) {
        LCD_Display(0, 1, "                   ");
        LCD_Display(0, 2, "                   ");
        LCD_Display(0, 1, "   XY coordinates");
        LCD_Display(0, 2, "X: ");
        lcd.print(x_coordinate);
        LCD_Display(11, 2, "Y: ");
        lcd.print(y_coordinate);
        delay(250);
      }
      // 馬達雙軸判斷fixto
      if ((xValue < 400 && yValue < 300) || words == '7') {
        xsymbol = '@';
        B_Manual = true;
        // 狀態顯示(移動中(綠)，print: move)
        digitalWrite(G_LED, HIGH);
        digitalWrite(R_LED, LOW);
        lcd.setCursor(8, 0);
        lcd.print("move    ");
        digitalWrite(xdirPin, HIGH);
        digitalWrite(ydirPin, LOW);
        digitalWrite(zdirPin, LOW);
        motor_ON();
      } else if ((xValue > 800 && yValue < 50) || words == '9') {
        ysymbol = '#';
        B_Manual = true;
        // 狀態顯示(移動中(綠)，print: move)
        digitalWrite(G_LED, HIGH);
        digitalWrite(R_LED, LOW);
        lcd.setCursor(8, 0);
        lcd.print("move     ");
        digitalWrite(xdirPin, LOW);
        digitalWrite(ydirPin, LOW);
        digitalWrite(zdirPin, LOW);
        motor_ON();
      } else if ((xValue < 400 && yValue > 800) || words == '1') {
        xsymbol = '$';
        B_Manual = true;
        // 狀態顯示(移動中(綠)，print: move)
        digitalWrite(G_LED, HIGH);
        digitalWrite(R_LED, LOW);
        lcd.setCursor(8, 0);
        lcd.print("move    ");
        digitalWrite(xdirPin, HIGH);
        digitalWrite(ydirPin, HIGH);
        digitalWrite(zdirPin, HIGH);
        motor_ON();
      } else if ((xValue > 800 && yValue > 800) || words == '3') {
        ysymbol = '%';
        B_Manual = true;
        // 狀態顯示(移動中(綠)，print: move)
        digitalWrite(G_LED, HIGH);
        digitalWrite(R_LED, LOW);
        lcd.setCursor(8, 0);
        lcd.print("move    ");
        digitalWrite(xdirPin, LOW);
        digitalWrite(ydirPin, HIGH);
        digitalWrite(zdirPin, HIGH);
        motor_ON();
      }
      // 馬達單軸判斷
      else if (xValue < 400 || words == '6') {
        xsymbol = '+';
        B_Manual = true;
        // 狀態顯示(移動中(綠)，print: move)
        digitalWrite(G_LED, HIGH);
        digitalWrite(R_LED, LOW);
        lcd.setCursor(8, 0);
        lcd.print("move     ");
        digitalWrite(xdirPin, HIGH);
        motor_ON();
      } else if (xValue > 800 || words == '4') {
        xsymbol = '-';
        B_Manual = true;
        // 狀態顯示(移動中(綠)，print: move)
        digitalWrite(G_LED, HIGH);
        digitalWrite(R_LED, LOW);
        lcd.setCursor(8, 0);
        lcd.print("move    ");
        digitalWrite(xdirPin, LOW);
        motor_ON();
      } else if (yValue < 300 || words == '8') {
        ysymbol = '-';
        B_Manual = true;
        // 狀態顯示(移動中(綠)，print: move)
        digitalWrite(G_LED, HIGH);
        digitalWrite(R_LED, LOW);
        lcd.setCursor(8, 0);
        lcd.print("move    ");
        digitalWrite(ydirPin, LOW);
        digitalWrite(zdirPin, LOW);
        motor_ON();
      } else if (yValue > 800 || words == '2') {
        ysymbol = '+';
        B_Manual = true;
        // 狀態顯示(移動中(綠)，print: move)
        digitalWrite(G_LED, HIGH);
        digitalWrite(R_LED, LOW);
        lcd.setCursor(8, 0);
        lcd.print("move    ");
        digitalWrite(ydirPin, HIGH);
        digitalWrite(zdirPin, HIGH);
        motor_ON();
      }
      // 再次按下搖桿，跳出搖桿模式
      else if (/*(buttonState == 0 && button == 1) || */(keys == 'm' && button == 1) || (words == '5' && button == 1)) {
        button = 0;
        // 直接離開
        modeReturn = true;
        digitalWrite(G_LED, LOW);
        digitalWrite(R_LED, HIGH);
        lcd.setCursor(6, 1);
        lcd.print("Stick OFF");
        modeLCD_display("", 2, 1, online, "      ", "return", "online");
        Serial1.println("Stick OFF");
        //Serial.println("press 'R' return or '5' continue, 'O' online, 'P' offline.");
        break;
      }
      // 如果按下'o'，則存儲位置
      else if (keys == 'o' &&  button == 1 || words == 'O') {
        B_x_axis[B_i] = x_coordinate;
        B_y_axis[B_i] = y_coordinate;
        // 模式E再次執行用
        //E_x_axis[B_i] = x_coordinate;
        //E_y_axis[B_i] = y_coordinate;
        lcd.setCursor(8, 0);
        lcd.print("N:     ");
        lcd.setCursor(10, 0);
        lcd.print(B_i);
        Serial.print("Current X: ");
        Serial.println(x_coordinate);
        Serial.print("Current Y: ");
        Serial.println(y_coordinate);
        button = 0;
        if (B_i < B_listSize)
          B_i++;
        else
          break;
      }
      // 如果按下's'，則切換顯示介面
      else if (keys == 's' && button == 1)
        B_display = !B_display;
      button = 1;
    }
  }
}

// 半自動模式(常規(紅)、藍))
void mode_C() {
  // LCD顯示
  lcd.clear();
  Serial1.println("# 當前為mode_C，半自動(Semi Automatic Mode)模式!");
  lcd.setCursor(0, 0);
  lcd.print("mode_C: Input");
  lcd.setCursor(0, 1);
  lcd.print("X:");
  lcd.print(xsymbol);
  lcd.setCursor(3, 1);
  lcd.print(xnum_01);
  lcd.print(xnum_02);
  lcd.print(".");
  lcd.print(xnum_03);
  lcd.print(xnum_04);
  lcd.setCursor(9, 1);
  lcd.print("Y:");
  lcd.print(ysymbol);
  lcd.setCursor(12, 1);
  lcd.print(ynum_01);
  lcd.print(ynum_02);
  lcd.print(".");
  lcd.print(ynum_03);
  lcd.print(ynum_04);
  lcd.setCursor(17, 1);
  lcd.print("(mm");
  modeLCD_display("", 2, 1, online, "      ", "return", "online");

  // 模式C_半自動模式啟用
  C_Semi_automatic = true;
}

// 自動模式
void mode_D() {
  Dnum_01 = 0;
  Dnum_02 = 0;
  lcd.clear();
  Serial1.println("# 當前為mode_D，自動(Automatic Mode)模式!");
  lcd.setCursor(0, 0);
  lcd.print("mode_D: Input");
  lcd.setCursor(0, 1);
  lcd.print("set frequency: ");
  lcd.print(Dnum_01);
  modeLCD_display("", 2, 1, online, "      ", "return", "      ");
  // 預先歸零
  digitalWrite(xdirPin, LOW);
  digitalWrite(ydirPin, LOW);
  digitalWrite(zdirPin, LOW);
  Correction_procedure(2);

  // 模式D_自動模式啟用
  D_Automatic = true;
}

// 點位自定義模式(搖桿移動定位)
void mode_E() {
  Enum_01 = 1;
  Enum_02 = 0;
  Enum_03 = 0;
  lcd.clear();
  Serial1.println("# 當前為mode_E，自定義(Point custom Mode)模式!");
  lcd.setCursor(0, 0);
  lcd.print("mode_E: Input ");
  lcd.setCursor(0, 1);
  lcd.print("Documented point: ");
  lcd.print(B_i - 1);
  lcd.setCursor(0, 2);
  lcd.print("set delay time: ");
  lcd.print(Enum_02);
  lcd.print(Enum_03);
  lcd.setCursor(18, 2);
  lcd.print("(s");
  modeLCD_display("", 2, 1, online, "      ", "return", "      ");
  for (int i = 1; i < B_i; i++) {
    Serial.print("Point ");
    Serial.print(i);
    Serial.print(": (");
    Serial.print(B_x_axis[i]);
    Serial.print(", ");
    Serial.print(B_y_axis[i]);
    Serial.println(")");
  }
  // 預先歸零
  digitalWrite(xdirPin, LOW);
  digitalWrite(ydirPin, LOW);
  digitalWrite(zdirPin, LOW);
  Correction_procedure(2);

  // 模式E_自定義模式啟用
  E_Point_custom = true;
}

// Developer Mode：開發者模式(up to date for 10/13)
// Update content：限位開關啟用與否(全禁止、單獨禁止)、檢查查看串口狀態、調整馬達速度(Speed)
// limit Switch：限位開關, Serial State：串口狀態, Step Delay：步數延遲
// 新增變數：limitenable, T_mode, motorDelay, delayenable, 變數變更：T_Test => T_Developer
void mode_T() {
  lcd.clear();
  LCD_Display(0, 0, "mode_T: Input");
  // 初始狀態設置
  T_mode = 0;
  LCD_Display(2, 1, "<");
  LCD_Display(4, 1, "limit switch");
  LCD_Display(17, 1, ">");
  LCD_Display(0, 2, "State:");
  // 讀取當前狀態
  if (T_Developer == 0)
    LCD_Display(7, 2, "None");
  else if (T_Developer == 1)
    LCD_Display(7, 2, "All ");
  else if (T_Developer == 2)
    LCD_Display(7, 2, "One ");
  else if (T_Developer == 3)
    LCD_Display(7, 2, "Two ");
  modeLCD_display("", 2, 1, online, "      ", "return", "      ");

  while (true) {
    char keys = keypad.getKey();
    if (keys == '<') {
      T_mode -= 1;
      // T_mode：" Step Delay "
      if (T_mode == 2) {
        LCD_Display(4, 1, " Step Delay ");
        // 读取当前步进延迟(for 10/27)
        if (delayenable == 4) {
          LCD_Display(7, 2, "200 ");
        } else if (delayenable == 3) {
          LCD_Display(7, 2, "1000");
        } else if (delayenable == 2) {
          LCD_Display(7, 2, "800 ");
        } else if (delayenable == 1) {
          LCD_Display(7, 2, "600 ");
        } else if (delayenable == 0) {
          LCD_Display(7, 2, "400 ");
        }
      }
      // T_mode："Serial State"
      else if (T_mode == 1) {
        LCD_Display(4, 1, "Serial State");
        // 增添读取串口状态(for 10/16)
        if (Serial1.available() <= 0)
          LCD_Display(7, 2, "true ");
        else if (Serial1.available() > 0)
          LCD_Display(7, 2, "false");
      }
      // T_mode："limit switch"(for 10/26)
      else {
        LCD_Display(4, 1, "limit switch");
        // 讀取當前狀態
        if (T_Developer == 0)
          LCD_Display(7, 2, "None");
        else if (T_Developer == 1)
          LCD_Display(7, 2, "All ");
        else if (T_Developer == 2)
          LCD_Display(7, 2, "One ");
        else if (T_Developer == 3)
          LCD_Display(7, 2, "Two ");
        T_mode = 3;
      }
    } else if (keys == '>') {
      T_mode += 1;
      if (T_mode == 1) {
        LCD_Display(4, 1, "Serial State");
        // 增添读取串口状态(for 10/16)
        if (Serial1.available() <= 0)
          LCD_Display(7, 2, "true ");
        else if (Serial1.available() > 0)
          LCD_Display(7, 2, "false");
      }
      else if (T_mode == 2) {
        LCD_Display(4, 1, " Step Delay ");
        // 读取当前步进延迟(for 10/27)
        if (delayenable == 4) {
          LCD_Display(7, 2, "200 ");
        } else if (delayenable == 3) {
          LCD_Display(7, 2, "1000");
        } else if (delayenable == 2) {
          LCD_Display(7, 2, "800 ");
        } else if (delayenable == 1) {
          LCD_Display(7, 2, "600 ");
        } else if (delayenable == 0) {
          LCD_Display(7, 2, "400 ");
        }
      }
      // T_mode："limit switch"(for 10/26)
      else {
        LCD_Display(4, 1, "limit switch");
        // 讀取當前狀態
        if (T_Developer == 0)
          LCD_Display(7, 2, "None");
        else if (T_Developer == 1)
          LCD_Display(7, 2, "All ");
        else if (T_Developer == 2)
          LCD_Display(7, 2, "One ");
        else if (T_Developer == 3)
          LCD_Display(7, 2, "Two ");
        T_mode = 0;
      }
    } else if (keys == 's') {
      // T_mode："limit switch"
      if (T_mode == 0) {
        // 讀取當前狀態
        if (T_Developer == 0)
          LCD_Display(7, 2, "None");
        else if (T_Developer == 1)
          LCD_Display(7, 2, "All ");
        else if (T_Developer == 2)
          LCD_Display(7, 2, "One ");
        else if (T_Developer == 3)
          LCD_Display(7, 2, "Two ");

        while (true) {
          char keys = keypad.getKey();
          // 狀態：None(無限制)、All(全限制)、One(X軸單軸限制)、Two(YZ軸限制)。
          if (keys == '<') {
            limitenable += 1;
            if (limitenable == 3) {
              LCD_Display(7, 2, "Two ");
              T_Developer = 3;
            } else if (limitenable == 2) {
              LCD_Display(7, 2, "One ");
              T_Developer = 2;
            } else if (limitenable == 1) {
              LCD_Display(7, 2, "All ");
              T_Developer = 1;
            } else {
              LCD_Display(7, 2, "None");
              limitenable = 0;
              T_Developer = 0;
            }
          } else if (keys == '>') {
            limitenable -= 1;
            if (limitenable == 1) {
              LCD_Display(7, 2, "All ");
              T_Developer = 1;
            } else if (limitenable == 2) {
              LCD_Display(7, 2, "One ");
              T_Developer = 2;
            } else if (limitenable == 3) {
              LCD_Display(7, 2, "Two ");
              T_Developer = 3;
            } else {
              LCD_Display(7, 2, "None");
              limitenable = 4;
              T_Developer = 0;
            }
          } else if (keys == 'm') {
            LCD_Display(0, 2, "State:              ");
            break;
          }
        }
      }
      // T_mode："Serial State"
      else if (T_mode == 1 || T_mode == -2) {
        if (Serial1.available() <= 0)
          LCD_Display(7, 2, "true ");
        else if (Serial1.available() > 0)
          LCD_Display(7, 2, "false");
      }
      // T_mode："Step Delay"
      else if (T_mode == -1 || T_mode == 2) {
        while (true) {
          char keys = keypad.getKey();
          // 狀態：Delay 200, Delay 400, Delay 600, Delay 800, Delay 1000。
          if (keys == '<') {
            delayenable += 1;
            if (delayenable == 4) {
              LCD_Display(7, 2, "200 ");
              motorDelay = 200;
            } else if (delayenable == 3) {
              LCD_Display(7, 2, "1000");
              motorDelay = 1000;
            } else if (delayenable == 2) {
              LCD_Display(7, 2, "800 ");
              motorDelay = 800;
            } else if (delayenable == 1) {
              LCD_Display(7, 2, "600 ");
              motorDelay = 600;
            } else {
              LCD_Display(7, 2, "400 ");
              delayenable = 0;
              motorDelay = 400;
            }
          } else if (keys == '>') {
            delayenable -= 1;
            if (delayenable == 1/* || delayenable == -4*/) {
              LCD_Display(7, 2, "600 ");
              motorDelay = 600;
            } else if (delayenable == 2/* || delayenable == -3*/) {
              LCD_Display(7, 2, "800 ");
              motorDelay = 800;
            } else if (delayenable == 3/* || delayenable == -2*/) {
              LCD_Display(7, 2, "1000");
              motorDelay = 1000;
            } else if (delayenable == 4/* || delayenable == -1*/) {
              LCD_Display(7, 2, "200");
              motorDelay = 200;
            } else {
              LCD_Display(7, 2, "400 ");
              delayenable = 5;
              motorDelay = 400;
            }
          } else if (keys == 'm') {
            LCD_Display(0, 2, "State:              ");
            break;
          }
        }
      }
    } else if (keys == 'm') {
      break;
    }
  }
}

// Changelog(韌體版本)
void mode_V() {
  lcd.clear();
  LCD_Display(0, 0, "mode_V: Input");
  // 當前版本
  LCD_Display(0, 1, "Version: ");
  lcd.print(updated_Version);
  // 更新日期
  LCD_Display(0, 2, "V_Date: ");
  lcd.print(updated_Date);
  modeLCD_display("", 2, 1, online, "      ", "return", "      ");
}