// 스텝 모터의 마이크로스테핑은 1/8 기준으로 한다. 

#include <SPI.h>
#include <SD.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
#include <Encoder.h>   // https://github.com/PaulStoffregen/Encoder

LiquidCrystal_I2C lcd(0x27, 16, 2);  // I2C LCD 객체 선언

#define encoderA 2        //엔코더 스위치의 A핀 - interrupt of arduino nano
#define encoderB 3        //엔코더 스위치의 B핀 - interrupt of arduino nano
#define encoderSW 4        //엔코더 스위치 핀, 현재 회로상에선 
#define piezo 5        //피에조 부저가 연결된 핀 - PWM
#define DCmotorPWM 9       //DC모터의 PWM 신호를 출력하는 핀

#define XDir A0           //X축(종이)의 스텝 신호 
#define XStep A1            //X축(종이)의 스텝 신호 
#define YDir A2               //Y축(펀쳐)의 스텝 신호 
#define YStep A3             //Y축(펀쳐)의 스텝 신호 
#define stepEN A4          //스텝모터의 Enable 신호 
#define XLimit A5          //X(종이)의 리밋 스위치    
#define YLimit A6           //Y(펀쳐)의 리밋 스위치
#define puncherLimitUP 11   //펀쳐의 리밋 스위치(위)
#define puncherLimitDN 12   //펀처의 리밋 스위치(아래)
#define DCmotorINB A9      //DC모터의 제어핀 B
#define DCmotorINA A10      //DC모터의 제어핀 A



String fileName[50] = {};   //파일이름을 저장할 변수(DXF포맷만)
String tempName;            //임시로 이름을 저장할 변수
byte numFile = 0;             //파일의 개수를 저장할 변수(파일의 총 갯수)
byte selectedNumFile;          //펀칭으로 선택된 한 파일의 위치를 파악하기 위한 변수 
byte menuState = 0;           //메뉴의 스테이지 상태 저장용 변수.
int oldPosition  = 0;
byte cursor = 0;                //LCD에 파일리스트를 표시할 때 커서의 위치를 관리하기 위한 변수. LCD표시와 상관없이 몇번째 파일을 선택하는지 표시한다. 
byte row = 0;                   //LCD에 파일리스트를 표시할 때 커서의 위치를 관리하기 위한 변수. LCD에서 몇번째 row인지 표시한다. 
byte tempCursor;                //LCD에 파일명을 표시할 때 커서의 위치를 관리하기 위한 변수
int i = 0, j = 0, k = 0, l=0, d[7];                        //여러 반복식에 임시로 쓰기 위한 변수,d는 DXF 포맷 로딩용으로 배열 선언.
char icon[3] = {0x2F, 0x2D, 0x7C}; // 아스키 코드  / , - , \ , | -LCD에 로딩중 표시를 위해 사용한다    
char circle[7] = {67, 73, 82 ,67, 76, 69, 10};     //아스키 코드 CIRCLE+LF - DXF포맷을 읽을때 CIRCLE 텍스트를 읽기 위해 사용한다
char Xsignal[4] = {32, 49 ,48, 10};                //아스키 코드 _10+LF - DXF포맷을 읽을때 X좌표를 읽기 위해 사용한다
char Ysignal[4] = {32, 50, 48, 10};                //아스키 코드 _20+LF - DXF포맷을 읽을때 Y좌표를 읽기 위해 사용한다  
boolean puncherUpFlag;                              //디바운싱 없이 펀쳐의 상태를 확실히 하기 위한 변수 .
boolean circleFlag = false;                         //DXF읽는 중에 CIRCLE 명령이 나왔는지 확인하는 플래그 변수 
boolean XFlag = false, YFlag = false;   
boolean XdirFactor = true, YdirFactor = true;     //XY 스텝모터의 방향 세팅용 변수. 

float punchPosX[700], punchPosY[700];             //펀칭할 위치좌표 [i] 숫자가 최대 펀치의 갯수가 된다 
float stepPosX, stepPosY;            
float defaultStepPosX= -85, defaulStepPosY = 66.7; //펀쳐의 최초 영점 좌표
float XFactor = 26.5, YFactor = 40;               //1mm 이동하는데 필요한 스텝값. 
float tempLocation;                                 //위치좌표 임시저장용 변수
int inChar;                                       //좌표값 읽을 때 char로 읽어서 string에 저장하는데 임시 char 저장용으로 쓰는 변수
int x = 0, y = 0;                                     //펀칭 위치좌표의 배열 순서에 쓰일 변수.                                 

boolean encoderFlagUp;          //엔코더에서 들어온 UP 신호 플래그(UP 행동후 false 전환)
boolean encoderFlagDown;        //엔코더에서 들어온 DOWN 신호 플래그(DOWN 행동후 false전환)
boolean SWpressedFlag = false;        //스위치가 눌렸는지 저장하는 플래그 변수 
boolean selectYes = true;          //Yes-No를 선택할 경우 선택값을 저장하는 변수 



#define G6  1568
#define C7  2093
#define E7  2637
#define G7  3136
#define B7  3951
#define C8  4186
#define D8  4699

//Mario main theme melody
int melody[] = {
  E7, E7, 0, E7, 0, C7, E7, 0,
  G7, 0, 0,  0, G6, 0, 0, 0
};
//Mario main them tempo
byte tempo = 12;


File myFile;
Encoder myEncoder(encoderA, encoderB);

void setup() {
  //전체 핀의 입출력 설정 
  pinMode(piezo, OUTPUT);
  pinMode(stepEN, OUTPUT);
  pinMode(XStep, OUTPUT);
  pinMode(XDir, OUTPUT);
  pinMode(YStep, OUTPUT);
  pinMode(YDir, OUTPUT);
  pinMode(DCmotorPWM, OUTPUT);
  pinMode(DCmotorINA, OUTPUT);
  pinMode(DCmotorINB, OUTPUT);

  pinMode(puncherLimitUP, INPUT);
  pinMode(puncherLimitDN, INPUT);
  pinMode(encoderSW, INPUT);
  pinMode(encoderA, INPUT);
  pinMode(encoderB, INPUT);
  pinMode(XLimit, INPUT);
  pinMode(YLimit, INPUT);

  digitalWrite(stepEN, HIGH);       //스텝모터의 전원을 꺼놓는다.

  playMelody();

  lcd.init();
  lcd.backlight();

          //부팅 화면 표시
  lcd.setCursor(3,0);
  lcd.print(F("Music Box"));
  lcd.setCursor(4,1);
  lcd.print(F("Puncher"));
  delay(1000);

  lcd.clear();

  lcd.setCursor(3,0);
  lcd.print(F("made by"));
  lcd.setCursor(2,1);
  lcd.print(F("Jonghun Lee"));
  delay(1000);

  lcd.clear();

  lcd.setCursor(2,0);
  lcd.print(F("Initializing"));
  lcd.setCursor(3,1);
  lcd.print(F("SD card..."));
  delay(1000);

  if (!SD.begin(10)) {        //SD카드의 CS핀을 10아두이노의 D10으로 설정. 
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print(F("SDinitialization"));
    lcd.setCursor(5,1);
    lcd.print(F("failed!"));
    for(i=0;i<5;i++){
      beepbeep();
    }
    while (1);
  }

  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print(F("initialization"));
  lcd.setCursor(6,1);
  lcd.print(F("done!"));
  beep();
  delay(200);
  lcd.clear();

  myFile = SD.open("/");    //루트 디렉토리를 연다. 
  readFileName(myFile, 0);    //SD 카드의 폴더/파일명을 읽어 저장하는 함수 

  //펀처가 올라간 상태에서 시작되어야 하므로 올라가 있지 않다면 위로 올린다.
  while(digitalRead(puncherLimitDN) == LOW){  
    digitalWrite(DCmotorINA, HIGH);    //펀처 업 동작 
    digitalWrite(DCmotorINB, LOW);
    analogWrite(DCmotorPWM, 255);
  }
  digitalWrite(DCmotorINA, LOW);    //브레이크 동작
  digitalWrite(DCmotorINB, LOW);
  analogWrite(DCmotorPWM, 255);

}



void loop() {


  switch (menuState) {
    case 0:       //파일 리스트를 LCD에 표시하고 선택한다
      readEncoder();

      //이하 LCD에 파일을 순서대로 리스트업하고 커서를 표시하는 구문
      if(row==0 && encoderFlagUp==true){
        cursor++;
        row++;
        beep();
        encoderFlagUp = false;
        lcd.clear();
      }
        if(row==0 && encoderFlagDown==true){
          if(cursor==0){                //예외.커서가 0 이하로 내려가지 못하게 한다. 
            beep();
            encoderFlagDown = false;
            lcd.clear();
          }
          else{
          cursor--;
          beep();
          encoderFlagDown = false;
          lcd.clear();
          }
        }

        if(row==1 && encoderFlagUp==true){
          if(cursor==numFile-1){
            beep();
            encoderFlagUp = false;
            lcd.clear();
          }
          else{
          cursor++;
          beep();
          encoderFlagUp = false;
          lcd.clear();
          }
        }
        if(row==1 && encoderFlagDown==true){
          beep();
          cursor--;
          row--;
          encoderFlagDown = false;
          lcd.clear();
        }
        //여기까지

        //이하 정리된 위치에 파일과 커서를 표시한다. 
        lcd.setCursor(0,row);
        lcd.print(F(">"));

        if(row==0){                   //커서가 위쪽이면 아래쪽으로 파일을 표시 
        lcd.setCursor(1,0);
        lcd.print(fileName[cursor]);
        lcd.setCursor(1,1);
        lcd.print(fileName[cursor+1]);
        }
        if(row==1){                   //커서가 아래쪽이면 위쪽부터 파일을 표시 
        lcd.setCursor(1,0);
        lcd.print(fileName[cursor-1]);
        lcd.setCursor(1,1);
        lcd.print(fileName[cursor]);
        }

        if(digitalRead(encoderSW)==HIGH){       //스위치가 눌리면 일단 기억한다. 
          SWpressedFlag = true;
        }

        if(digitalRead(encoderSW)==LOW && SWpressedFlag==true){  //스위치가 안 눌리면, 눌렸는지 확인하고 그렇다면 아래 구문을 실행
          selectedNumFile = cursor;   
          beep();
          SWpressedFlag = false;
          lcd.clear();
          menuState++;
        }

    break;      //case 0의 끝

    case 1:       //선택한 파일을 로드할지 표시하고 선택한다 

      readEncoder();

      if(encoderFlagUp==true||encoderFlagDown==true){   //엔코더 스위치가 눌렸으면 YES <-> NO 전환하고 비프음 울린다. 
        selectYes = !selectYes;
        beep();
        encoderFlagUp = false;
        encoderFlagDown = false;
        lcd.clear();
      }

      lcd.setCursor(0,0);                               //파일명과 YES / NO 선택 표시 
      lcd.print(fileName[selectedNumFile]);
      lcd.setCursor(0,1);
      lcd.print(F("LOAD?"));
      lcd.setCursor(7,1);
      lcd.print(F("YES"));
      lcd.setCursor(12,1);
      lcd.print(F("NO"));

      if(selectYes==true){
        lcd.setCursor(6,1);
        lcd.print(F(">"));
        lcd.setCursor(10,1);
        lcd.print(F("<"));
      }
      else{
        lcd.setCursor(11,1);
        lcd.print(F(">"));
        lcd.setCursor(14,1);
        lcd.print(F("<"));
      }

      if(digitalRead(encoderSW)==HIGH){
        SWpressedFlag = true;
      }

      if(digitalRead(encoderSW)==LOW && SWpressedFlag == true){       //YES 선택중에 엔코더 스위치가 눌리면 비프음 울리고 다음 메뉴로 넘어간다.
        if(selectYes==true){
          beep();
          menuState++;
          i = 0;
          j = 0;
        }
        else{
          cursor = 0;             //NO 선택시 이전 메뉴로 돌아가고 cursor와 row를 초기화해준다.   
          row = 0;
          beepbeep();
          menuState--;
        }
        lcd.clear();
        SWpressedFlag = false;
      }
    break;        //case 1의 끝 

    case 2:       //선택한 파일을 로드해서 펀칭 데이터를 스캔하고 저장한다. 

      lcd.setCursor(2,0);
      lcd.print(F("LOADING...."));
      lcd.setCursor(13,0);
      load_bar();

      tempCursor = fileName[selectedNumFile].length();     //파일명을 LCD 가운데에 위치하도록 길이를 재서 커서 위치를 계산한다
      tempCursor = (16 - tempCursor) / 2;
      lcd.setCursor(tempCursor,1);
      lcd.print(fileName[selectedNumFile]);

      myFile = SD.open(fileName[selectedNumFile]);    //선택된 파일을 SD 카드에서 오픈. 

      if(myFile){           //선택된 파일의 내용을 읽는다. 
        while(myFile.available()){

          if(circleFlag==false){   //circleFlag = false 인 경우, circle만 체크한다. 
            d[i] = myFile.read(); //한글자씩 읽음
            if(d[i]==circle[i]){  //첫글자가 C라면 아래 구문 실행.
            i++;    
            }
            else{
              i=0;
            }
            if(i==6){       //6번째 글자까지 동일하면 Circle=true
              circleFlag = true;
              i=0;
            }
          }
 
          else{                       //이하 CircleFlag == true 인 상황. 

            if(XFlag==true){

              if(YFlag==true){        //전부 true 이므로 전 과정이 끝난 상황. 초기화한다. 
                XFlag = false;
                YFlag = false;
                circleFlag = false;
                i = 0;
              } 

              else{                   //XFlag = true이고 YFlag만 false인 상황. Y좌표를 읽는다. 
                d[i] = myFile.read();   //한글자씩 읽음.

                if(d[i]==Ysignal[i]){  //첫글자가 _(Space)라면 아래 구문 실행.
                  i++;    
                }
                else{
                  i=0;
                }

                if(i==3){       //3번째 글자까지 동일하면 YFlag=true
                  YFlag = true;
                  ////좌표 읽기 
                  punchPosY[y] = readLocation();
                  y++;
                  i=0;
                }
              }      
            }

            else{                      //XFlag = false 인 상황, X좌표를 읽는다. 
              d[i] = myFile.read();   //한글자씩 읽음.
              if(d[i]==Xsignal[i]){  //첫글자가 _(Space)라면 아래 구문 실행.
              i++;    
              }
              else{
                i=0;
              }

              if(i==3){       //3번째 글자까지 동일하면 XFlag=true
                XFlag = true;
                punchPosX[x] = readLocation();
                x++;
                i=0;
              }
            }
          }//이상 CircleFlag == true 인 상황. 
        }
        myFile.close();
      }
      else{                       //파일이 안 열릴 때 에러 표시 
        lcd.clear();
        lcd.setCursor(tempCursor,0);
        lcd.print(fileName[selectedNumFile]);
        lcd.setCursor(0,1);
        lcd.print(F("---OPEN ERROR---"));
        for(i=0;i<5;i++){
          beepbeep();
        }
        lcd.clear();
        menuState = 0;
        break;
      }

      lcd.clear();
      menuState++;

    break;

    case 3:           //menuState 2번에서 이어받은 좌표값을 X 좌표 기준으로 정렬한다. 
      beep();
      lcd.setCursor(2,0);
      lcd.print(F("SORTING...."));
      lcd.setCursor(13,0);

      for(i=0; i<(x-1); i++) {
        for(j=0; j<(x-(i+1)); j++) {
          if(punchPosX[j] > punchPosX[j+1]) {
            tempLocation = punchPosX[j];
            punchPosX[j] = punchPosX[j+1];
            punchPosX[j+1] = tempLocation;

            tempLocation = punchPosY[j];
            punchPosY[j] = punchPosY[j+1];
            punchPosY[j+1] = tempLocation;

            lcd.setCursor(13,0);
            load_bar();
          }
        }
      }

      lcd.setCursor(2,0);
      lcd.print(F("COMPLETED.."));
      lcd.setCursor(13,0);

      beep();
      menuState++;
      lcd.clear();
    break;
    
    case 4:   //case 2: 와 비슷하다. 펀칭 데이터를 표시하고 펀칭할지 물어본다. NO를 선택하면 파일 선택 메뉴로 돌아간다. 

      readEncoder();
      if(encoderFlagUp==true||encoderFlagDown==true){   //엔코더 스위치가 눌렸으면 YES <-> NO 전환하고 비프음 울린다. 
        selectYes = !selectYes;
        beep();
        encoderFlagUp = false;
        encoderFlagDown = false;
        lcd.clear();
      }



      lcd.setCursor(0,0);                             //x개의 펀치 데이터가 있음을 표시한다. 
      lcd.print(x);
      lcd.print(F("DATA TO PUNCH"));
      lcd.setCursor(0,1);
      lcd.print(F("PUNCH?"));
      lcd.setCursor(8,1);
      lcd.print(F("YES"));
      lcd.setCursor(13,1);
      lcd.print(F("NO"));

      if(selectYes==true){
        lcd.setCursor(7,1);
        lcd.print(F(">")); 
        lcd.setCursor(11,1);
        lcd.print(F("<"));
      }
      else{
        lcd.setCursor(12,1);
        lcd.print(F(">"));
        lcd.setCursor(15,1);
        lcd.print(F("<"));
      }

      if(digitalRead(encoderSW)==HIGH){
        SWpressedFlag = true;
      }

      if(digitalRead(encoderSW)==LOW && SWpressedFlag == true){       //YES 선택중에 엔코더 스위치가 눌리면 비프음 울리고 다음 메뉴로 넘어간다.
        if(selectYes==true){
          beep();
          menuState++;
          i = 0;
          j = 0;
        }
        else{
          cursor = 0;             //NO 선택시 파일 선택 메뉴로 돌아가고 cursor와 row를 초기화해준다.   
          row = 0;
          beepbeep();
          menuState=0;
        }
        lcd.clear();
        SWpressedFlag = false;
      }
    break;

    case 5:     //센서 영점을 잡고 펀칭할 준비를 한다. 

    //XDir = LOW 는 종이를 집어넣는 방향으로 움직인다.
    //YDir = HIGH 는 펀쳐가 왼쪽으로 움직인다. 
    //모터에 따라 반대가 될 수도 있다
    
    digitalWrite(stepEN, LOW);        //스텝모터를 켠다. 

    digitalWrite(YDir, YdirFactor);         //펀처를 센서에서 벗어나게 오른쪽으로 움직임 
    while(digitalRead(YLimit)==true){
      digitalWrite(YStep, HIGH);
      delay(1);
      digitalWrite(YStep, LOW);
      delay(1);
    }
    digitalWrite(YDir, !YdirFactor);    //펀쳐를 센서가 작동할때까지 왼쪽으로 움직임 
    while(digitalRead(YLimit)==false){
      digitalWrite(YStep, HIGH);
      delay(1);
      digitalWrite(YStep, LOW);
      delay(1);
    }

    stepPosX = defaultStepPosX; //초기 위치 지정. 
    stepPosY = defaulStepPosY;

    beep();
    lcd.setCursor(0,0);
    lcd.print(F("--Insert Paper--"));


    while(digitalRead(XLimit)==true){   //만약 종이가 이미 들어있으면 후진한다. 
      digitalWrite(XDir, !XdirFactor);
      digitalWrite(XStep, HIGH);
      delay(1);
      digitalWrite(XStep, LOW);
      delay(1);
    }
    
    digitalWrite(XDir, XdirFactor);            //종이가 감지될때까지 전진한다. 
    while(digitalRead(XLimit)==false){
      digitalWrite(XStep, HIGH);
      delay(1);
      digitalWrite(XStep, LOW);
      delay(1);
    }
    lcd.clear();
    


    for(j=0;j<x;j++){
      stepMove(punchPosX[j],punchPosY[j]);
      lcd.setCursor(0,0);
      lcd.print(j+1);
      lcd.print(F("/"));
      lcd.print(x);
      lcd.print(F("PUNCHING.."));
      punch();
    }

    lcd.clear();   
    menuState++;
    break;

    case 6:
    stepMove(50,5);
    for(int i=0;i<4;i++){
      beepbeep();
    }
    menuState++;

    case 7:
    lcd.setCursor(0,0);
    lcd.print(F("Cut the paper"));
    lcd.setCursor(0,1);
    lcd.print(F("and Press Switch"));
    digitalWrite(stepEN, HIGH);   //스텝모터를 끈다. 


    if(digitalRead(encoderSW)==HIGH){
      SWpressedFlag = true;
    }
    if(digitalRead(encoderSW)==LOW && SWpressedFlag == true){   
      menuState++;
      SWpressedFlag = false;
      lcd.clear();
    }
    break;

    case 8:

    lcd.setCursor(0,0);
    lcd.print(F("All done."));

    for(j=0;j<x;j++){   //기존 데이터 초기화
      punchPosX[j]=NULL;
      punchPosY[j]=NULL;
    }
    x=0; y=0;           //기존 데이터 초기화
    playMelody();
    lcd.clear();
    menuState = 0;
    break;
  }
}


void readFileName(File dir, int numTabs) {
  while (true) {
    File entry =  dir.openNextFile();       //더 이상 파일이나 폴더가 없으면 종료
    if (! entry) {                          // no more files
      break;
    }
    tempName = entry.name();            //파일or폴더 이름을 tempName에 임시저장
    if(tempName.endsWith(".DXF")){       //끝부분이 .DXF로 끝나는 파일만 
      fileName[numFile] = tempName;     //fileName 변수에 저장한다. 
      numFile++;;
    }
    entry.close();
  }
  tempName = "";                      //tempName초기화
}

void readEncoder(){   //엔코더를 오른쪽으로 돌리는 것을 UP으로 설정한다. 
  int newPosition = myEncoder.read();

  if ((newPosition+1) < oldPosition) {      //값의 변화가 생기면 오른 값인지 내린 값인지 비교하여 Flag 설정. 
    encoderFlagUp = true;                   // +1 과 -1을 해주는 이유는 분해능 때문에 3번씩 반복되는것을 막기 위함  
  }
  if ((newPosition-1) > oldPosition) {
    encoderFlagDown = true;   
  }
oldPosition = newPosition;
  if(oldPosition > 32760 || oldPosition < -32760){
    oldPosition = 0;        //int 변수의 범위를 벗어나지 않도록 고정. 
  }
}

void playMelody(){
 int size = sizeof(melody) / sizeof(int);
    for (int thisNote = 0; thisNote < size; thisNote++) {
 
      // to calculate the note duration, take one second
      // divided by the note type.
      //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
      int noteDuration = 1000 / tempo;
 
      buzz(piezo, melody[thisNote], noteDuration);
 
      // to distinguish the notes, set a minimum time between them.
      // the note's duration + 30% seems to work well:
      int pauseBetweenNotes = noteDuration * 1.30;
      delay(pauseBetweenNotes);
 
      // stop the tone playing:
      buzz(piezo, 0, noteDuration);
    }
}

void buzz(int targetPin, long frequency, long length) {
  long delayValue = 1000000 / frequency / 2; // calculate the delay value between transitions
  //// 1 second's worth of microseconds, divided by the frequency, then split in half since
  //// there are two phases to each cycle
  long numCycles = frequency * length / 1000; // calculate the number of cycles for proper timing
  //// multiply frequency, which is really cycles per second, by the number of seconds to
  //// get the total number of cycles to produce
  for (long k = 0; k < numCycles; k++) { // for the calculated length of time...
    digitalWrite(targetPin, HIGH); // write the buzzer pin high to push out the diaphram
    delayMicroseconds(delayValue); // wait for the calculated delay value
    digitalWrite(targetPin, LOW); // write the buzzer pin low to pull back the diaphram
    delayMicroseconds(delayValue); // wait again or the calculated delay value
  } 
}

void beepbeep(){
  tone(piezo, B7, 50);
  delay(100);
  tone(piezo, B7, 50);
  delay(500);
}

void beep(){
  tone(piezo, C8, 30);
  delay(40);
}

float readLocation(){
  inChar = myFile.read();
  inChar = 0;

  while(inChar != 10){        //10 = Line Finish
    inChar = myFile.read();
    tempName += (char)inChar;   //tempName은 파일명 읽을 때 사용한 임시변수
  }

  tempLocation = tempName.toFloat();    //String 을 float로 변환해서 tempLocation에 저장한다. 
  tempLocation = tempLocation * 25.4;   //inch to mm변환. 
  tempName = "";      //tempName 초기화. 

  return tempLocation;  
}

void load_bar(){
  lcd.print(icon[l]);
  k++;
  if(k>50){     //k 가 50이 넘어갈때마다 l을 1 올려 아이콘을 변경한다
    k = 0;
    l++;
  }
  if(l>2){
    l=0;
  }
}

void stepMove(float toMoveX, float toMoveY){
  float Xdistance = toMoveX - stepPosX;
  float Ydistance = toMoveY - stepPosY;

  for(i = Xdistance*XFactor;i>0;i--){
    digitalWrite(XDir, XdirFactor);
    digitalWrite(XStep, HIGH);
    delayMicroseconds(300);
    digitalWrite(XStep, LOW);
    delayMicroseconds(300);
  }

  if(Ydistance>0){
    for(i = Ydistance*YFactor;i>0;i--){
      digitalWrite(YDir, !YdirFactor);          //펀처가 오른쪽으로 이동
      digitalWrite(YStep, HIGH);
      delayMicroseconds(200);
      digitalWrite(YStep, LOW);
      delayMicroseconds(200);
    }
  }
  else{
    for(i = Ydistance*YFactor;i<0;i++){
      digitalWrite(YDir, YdirFactor);         //펀처가 왼쪽으로 이동
      digitalWrite(YStep, HIGH);
      delayMicroseconds(200);
      digitalWrite(YStep, LOW);
      delayMicroseconds(200);
    }
  }

  stepPosX = toMoveX;
  stepPosY = toMoveY;
}

void punch(){
  while(digitalRead(puncherLimitUP) == LOW){
    digitalWrite(DCmotorINA, LOW);    //펀처 다운 동작 
    digitalWrite(DCmotorINB, HIGH);
    analogWrite(DCmotorPWM, 255);
  }

  while(digitalRead(puncherLimitDN) == LOW){
    digitalWrite(DCmotorINA, HIGH);    //펀처 업 동작 
    digitalWrite(DCmotorINB, LOW);
    analogWrite(DCmotorPWM, 255);
  }

  digitalWrite(DCmotorINA, LOW);    //브레이크 동작
  digitalWrite(DCmotorINB, LOW);
  analogWrite(DCmotorPWM, 255);
}