'=====================================================================
' M804 - 외경(보스/원기둥) 중심 잡기  (바깥쪽에서 X0 Y0)
' WP-500 V6 유선 3D 터치 프로브 + Mach3 Mill 용
' ※ 한글 주석은 Mach3 VB 편집기에서 깨져 보일 수 있으나 실행에는 무영향.
'---------------------------------------------------------------------
' 사용법
'   1. 볼을 보스 중심 "위쪽"으로 대략 옮깁니다 (윗면에서 몇 mm 위).
'      이 지점이 시작점이 됩니다.
'   2. 아래 BOSS_DIA 에 대략적인 바깥지름을 넣습니다.
'   3. MDI 창에:  M804
'   4. 프로브가 옆으로 나가 내려간 뒤 +X -X +Y -Y 네 면을 바깥에서
'      찍고, 진짜 중심으로 복귀해 X0 Y0 을 설정합니다.
'
'   모든 이동은 시작점 기준입니다. 따라서 보스는 시작점에서
'   BOSS_DIA/2 + CLEAR_XY 안에 있어야 합니다.
'=====================================================================

'------------------------- 설정 (여기를 수정) --------------------------
TIP_DIA      = 3.0      ' 스타일러스 볼 지름 [mm]
BOSS_DIA     = 30.0     ' 보스의 대략적인 바깥지름 [mm]
CLEAR_XY     = 6.0      ' 보스 바깥으로 비켜나가는 여유 거리 [mm]
Z_DEPTH      = 5.0      ' 보스 옆에서 시작 Z 보다 내려가는 깊이 [mm]
FAST_F       = 200      ' 1차 탐색 이송속도 [mm/min]
SLOW_F       = 15       ' 2차 정밀 측정 이송속도 [mm/min]
RETRACT      = 1.0      ' 두 패스 사이 후퇴량 [mm]
AVG_PASS     = 1        ' 저속 정밀 측정 반복 횟수 [회] - 2~3 이면 반복오차가 줄고, 그만큼 느려진다
REPEAT_PASS  = 1        ' 1 = 찾은 중심선에서 X 를 한 번 더 측정(권장)
USE_DRO      = 0        ' 0 = #2000~ 변수 사용(권장) | 1 = DRO 읽기
PROBE_LED    = 825      ' 프로브 입력 OEM LED 번호
PROBE_CHECK  = 1        ' 1 = 움직이기 전에 프로브를 손으로 눌러 신호를 확인한다 (강력 권장)
                        '     장비·프로브를 연결하지 않은 상태에서 실행하면 여기서 걸러진다
MAX_TRAVEL   = 100      ' 한 번의 프로빙이 갈 수 있는 절대 상한 [mm] - 설정 실수로 폭주하는 것을 막는다
DRY_RUN      = 0        ' 1 = 장비 없이 흐름만 확인. G코드를 내보내지 않고 기록만 하며
                        '     축은 움직이지 않고 영점도 잡지 않는다. 숫자는 가상값이다
DRY_STEP     = 5.0      ' DRY_RUN 일 때 가상 트립까지의 거리 [mm] - 계산이 돌아가게만 하는 값
'---------------------------------------------------------------------

gErr = 0                                ' 전역 에러 플래그 (0=정상, 1=에러 발생)
gProbeOK = 0                     ' 프로브 응답 확인 여부 (실행당 1회)
gLog = ""                        ' DRY_RUN 기록
gLogN = 0
R      = TIP_DIA / 2.0                  ' 볼 반지름
OUT    = BOSS_DIA / 2.0 + CLEAR_XY      ' 시작점에서 옆으로 비켜나갈 거리
SEARCH = CLEAR_XY * 2.0                 ' 보스를 향해 전진할 최대 탐색 거리

Emit "G21"                       ' 단위를 mm 로
Emit "G90"                       ' 절대좌표 모드로
WaitIdle                       ' 위 명령이 끝날 때까지 대기

' 시작 전 점검 : 프로브 입력(LED)이 이미 켜져 있지 않은지 확인한다.
' 정상이라면 볼이 허공에 있어 LED 는 꺼짐(0)이어야 한다. 켜져 있다면
' 배선 오류 / Active-Low 설정 반대 / 볼이 눌린 상태라는 뜻이고,
' 그대로 G31 을 실행하면 접촉을 감지하지 못한 채 계속 밀고 들어가
' 프로브가 부러질 수 있으므로 시작 전에 걸러낸다.
If DRY_RUN = 0 And GetOemLed(PROBE_LED) <> 0 Then
  MsgBox "Probe input is ALREADY active. Check wiring / Active-Low."
  End                            ' 매크로 즉시 종료
End If

' 시작점(보스 위 안전 위치) 좌표 기억 - 이후 모든 이동의 기준
sx = GetOemDro(800)              ' 시작 X
sy = GetOemDro(801)              ' 시작 Y
sz = GetOemDro(802)              ' 시작 Z (안전 높이)

'---------------- X 두 면 : 바깥에서 안쪽으로 찍기 --------------------
Message "M804 : probing X+ side ..."    ' 화면 하단 상태줄에 진행 표시
x1 = ProbeSide(0, 1, sx - OUT, sy)      ' X- 쪽 바깥으로 나가 내려간 뒤, X+ 방향(보스 쪽)으로 프로빙 -> x1
' 방금 프로빙이 실패했으면(gErr=1) 여기서 즉시 멈춘다.
' 이유 : 실패를 무시하고 계속하면 틀린 좌표로 영점이 잡히고,
' 그 영점으로 가공하면 공구·공작물 충돌 사고로 이어지기 때문.
If gErr <> 0 Then                ' 에러 발생 시 (접촉 실패 등)
  Call GoHomePos(sx, sy, sz)     ' 시작점으로 안전 복귀 후
  Message "M804 aborted."        ' 중단 안내 표시
  End                            ' 매크로 즉시 종료
End If

Message "M804 : probing X- side ..."
x2 = ProbeSide(0, -1, sx + OUT, sy)     ' 반대편 : X+ 쪽 바깥에서 X- 방향으로 프로빙 -> x2
' 방금 프로빙이 실패했으면(gErr=1) 여기서 즉시 멈춘다.
' 이유 : 실패를 무시하고 계속하면 틀린 좌표로 영점이 잡히고,
' 그 영점으로 가공하면 공구·공작물 충돌 사고로 이어지기 때문.
If gErr <> 0 Then
  Call GoHomePos(sx, sy, sz)
  Message "M804 aborted."
  End
End If

xc = (x1 + x2) / 2.0             ' X 중심 = 두 접촉점의 한가운데 (볼 반지름 상쇄)
dx = (x2 - x1) - TIP_DIA         ' 외경 지름 = 두 볼중심 간격 - 볼 지름 (바깥에서 쟀으므로 빼기)

'---------------- Y 두 면 : 찾은 X 중심에서 찍기 ----------------------
Message "M804 : probing Y+ side ..."
y1 = ProbeSide(1, 1, xc, sy - OUT)      ' 찾은 X 중심(xc)에서 Y- 쪽 바깥으로 나가 Y+ 방향 프로빙 -> y1
' 방금 프로빙이 실패했으면(gErr=1) 여기서 즉시 멈춘다.
' 이유 : 실패를 무시하고 계속하면 틀린 좌표로 영점이 잡히고,
' 그 영점으로 가공하면 공구·공작물 충돌 사고로 이어지기 때문.
If gErr <> 0 Then
  Call GoHomePos(sx, sy, sz)
  Message "M804 aborted."
  End
End If

Message "M804 : probing Y- side ..."
y2 = ProbeSide(1, -1, xc, sy + OUT)     ' 반대편에서 Y- 방향 프로빙 -> y2
' 방금 프로빙이 실패했으면(gErr=1) 여기서 즉시 멈춘다.
' 이유 : 실패를 무시하고 계속하면 틀린 좌표로 영점이 잡히고,
' 그 영점으로 가공하면 공구·공작물 충돌 사고로 이어지기 때문.
If gErr <> 0 Then
  Call GoHomePos(sx, sy, sz)
  Message "M804 aborted."
  End
End If

yc = (y1 + y2) / 2.0             ' Y 중심
dy = (y2 - y1) - TIP_DIA         ' Y 지름

'---------------- 선택 : 진짜 중심선에서 X 재측정 --------------------
' 1차 X 패스는 시작점이 중심에서 어긋난 현(chord) 위라 지름이 작게
' 나옵니다. 중심선(Y=yc)에서 다시 재면 실제 지름이 나옵니다.
' 중심 좌표 자체는 어느 쪽이든 정확합니다.
If REPEAT_PASS = 1 Then          ' 설정에서 재측정을 켰을 때만
  Message "M804 : re-probing X on the centre line ..."
  x1 = ProbeSide(0, 1, xc - OUT, yc)    ' 중심선 위에서 X 두 면 재측정
  ' 방금 프로빙이 실패했으면(gErr=1) 여기서 즉시 멈춘다.
  ' 이유 : 실패를 무시하고 계속하면 틀린 좌표로 영점이 잡히고,
  ' 그 영점으로 가공하면 공구·공작물 충돌 사고로 이어지기 때문.
  If gErr <> 0 Then
    Call GoHomePos(sx, sy, sz)
    Message "M804 aborted."
    End
  End If
  x2 = ProbeSide(0, -1, xc + OUT, yc)
  ' 방금 프로빙이 실패했으면(gErr=1) 여기서 즉시 멈춘다.
  ' 이유 : 실패를 무시하고 계속하면 틀린 좌표로 영점이 잡히고,
  ' 그 영점으로 가공하면 공구·공작물 충돌 사고로 이어지기 때문.
  If gErr <> 0 Then
    Call GoHomePos(sx, sy, sz)
    Message "M804 aborted."
    End
  End If
  xc = (x1 + x2) / 2.0           ' X 중심 갱신
  dx = (x2 - x1) - TIP_DIA       ' 이번 값이 실제 지름
End If

' 안전 높이로 올라간 뒤 측정된 중심으로 이동
Emit "G90 G0 Z" & sz             ' 먼저 Z 를 시작 높이로 (충돌 방지)
WaitIdle
Emit "G90 G0 X" & xc & " Y" & yc ' 보스 중심 위로 이동
WaitIdle

' 현 위치(보스 중심)를 X0 Y0 으로 설정
SetZero 800, GetOemDro(800) - xc   ' X DRO 재설정 -> 중심=0
SetZero 801, GetOemDro(801) - yc   ' Y DRO 재설정
WaitIdle                       ' DRO 반영 대기

' 측정 결과를 팝업으로 보고 (Chr(13) = 줄바꿈)
Done "Boss centre set to X0 Y0." & Chr(13) & "Measured dia X = " & dx & " mm" & Chr(13) & "Measured dia Y = " & dy & " mm"
Message "M804 done : boss centre X0 Y0"    ' 상태줄에도 완료 표시
DryReport "M804"
End                              ' 매크로 정상 종료

'=====================================================================
'  보조 루틴
'=====================================================================

'---------------------------------------------------------------------
' DRY_RUN 문지기 : 기계로 나가는 출구를 한곳으로 모은다.
'
' 매크로가 바깥 세상에 영향을 주는 길은 둘뿐이다 - G코드를 보내는 것(Code)과
' 영점 DRO 를 쓰는 것(SetOemDro). 그 둘만 막으면 나머지 계산은 아무리 돌려도
' 안전하다. 그래서 모든 호출을 Emit / SetZero 로 바꿔 두었다.
'---------------------------------------------------------------------
Sub Emit(s)
  If DRY_RUN = 1 Then
    ' 로그가 끝없이 길어지면 완료창에 담기지 않으므로 앞부분만 남긴다
    If gLogN < 40 Then
      gLog = gLog & s & Chr(13)
      gLogN = gLogN + 1
    ElseIf gLogN = 40 Then
      gLog = gLog & "... (이하 생략)" & Chr(13)
      gLogN = gLogN + 1
    End If
  Else
    Code s
  End If
End Sub

' 영점 설정 - DRY_RUN 에서는 절대 쓰지 않는다.
' 가상 좌표로 X0 Y0 을 잡아 버리면 진짜 영점인 줄 알고 가공하게 되므로,
' 이 모드의 가장 큰 위험이 바로 이 지점이다.
Sub SetZero(n, v)
  If DRY_RUN = 1 Then
    gLog = gLog & "[DRY RUN] SetOemDro(" & n & ") 생략 - 영점을 잡지 않았습니다" & Chr(13)
  Else
    Call SetOemDro(n, v)
  End If
End Sub

' 트립 좌표 읽기 - DRY_RUN 에서는 현재 위치에서 DRY_STEP 만큼 떨어진 가상값.
' 실제로 움직이지 않으므로 모든 축이 시작점을 기준으로 계산된다.
Function ReadTrip(i, sgn)
  If DRY_RUN = 1 Then
    ReadTrip = GetOemDro(800 + i) + sgn * DRY_STEP
  Else
    ReadTrip = TripPos(i)
  End If
End Function

' DRY_RUN 완료 보고 - 진짜 측정과 눈으로 구분되어야 한다
Sub DryReport(mac)
  If DRY_RUN <> 1 Then Exit Sub
  MsgBox "[DRY RUN] " & mac & Chr(13) & Chr(13) & "Nothing moved. No zero was set. Numbers are synthetic." & Chr(13) & Chr(13) & gLog
  Message "[DRY RUN] " & mac & " - nothing moved"
End Sub

' 완료 보고 - DRY_RUN 에서는 아무것도 하지 않았으므로 이 창을 띄우면 안 된다.
' 종전에 "Bore centre set to X0 Y0" 이 그대로 떠서, 영점을 잡은 것처럼 보였다.
Sub Done(s)
  If DRY_RUN = 1 Then Exit Sub    ' DryReport 가 대신 사실대로 보고한다
  MsgBox s
End Sub

' 이동이 완전히 끝날 때까지 대기.
' G31 직후 좌표를 읽으려면 기계가 완전히 멈춘 뒤여야 하기 때문.
Sub WaitIdle()
  Sleep(200)                     ' 0.2초 기본 대기 (명령 전달 시간)
  While IsMoving()               ' 축이 아직 움직이는 동안
    Sleep(20)                    ' 0.02초씩 기다리며 반복 확인
  Wend
End Sub

' 에러 시 시작점으로 안전 복귀 (Z 먼저 올리고 XY 이동 - 충돌 방지 순서)
Sub GoHomePos(px, py, pz)
  Emit "G90 G0 Z" & pz           ' 1) 안전 높이로 상승
  WaitIdle
  Emit "G90 G0 X" & px & " Y" & py   ' 2) 그 다음 수평 복귀
  WaitIdle
End Sub

' 축 번호(0/1/2) -> 축 이름("X"/"Y"/"Z") 변환.
' 이유 : G코드 명령은 "G31 X-12" 처럼 축 문자로 조립해야 하는데,
' 계산 로직은 숫자 번호가 편하므로 둘을 이어 주는 다리 역할.
Function AxisName(i)
  If i = 0 Then AxisName = "X"
  If i = 1 Then AxisName = "Y"
  If i = 2 Then AxisName = "Z"
End Function

' 트립(접촉 순간) 좌표 읽기
' 기본(USE_DRO=0)은 Mach3 가 G31 때 자동 기록하는 #2000~ 변수 사용
Function TripPos(i)
  If USE_DRO = 1 Then
    TripPos = GetOemDro(800 + i) ' 정지 후 DRO 에서 읽기 (구형 설정용)
  Else
    TripPos = GetVar(2000 + i)   ' G31 트립 변수에서 읽기 (권장)
  End If
End Function

'---------------------------------------------------------------------
' 한 면 측정 절차 (보스 전용) :
'   1) 안전 Z 에서 목표점(gx,gy)으로 수평 이동
'   2) Z_DEPTH 만큼 하강 (보스 옆면 높이로)
'   3) 보스를 향해 프로빙
'   4) 다시 안전 Z 로 상승
' 반환 : 트립(접촉) 좌표
'---------------------------------------------------------------------
Function ProbeSide(i, sgn, gx, gy)
  Dim t                          ' 트립 좌표 임시 저장
  ProbeSide = 0                  ' 반환값 초기화

  Emit "G90 G0 Z" & sz           ' 항상 안전 높이로 올린 뒤 수평 이동 (보스 충돌 방지)
  WaitIdle
  Emit "G90 G0 X" & gx & " Y" & gy   ' 측정 시작 위치로 이동
  WaitIdle
  Emit "G91 G0 Z" & (-Z_DEPTH)   ' 보스 옆에서 측정 깊이로 하강
  WaitIdle
  Emit "G90"                     ' 절대좌표 모드 복귀

  t = ProbeAxis(i, sgn, SEARCH)  ' 보스 면을 향해 프로빙

  Emit "G91 G0 Z" & Z_DEPTH      ' 측정 후 다시 상승
  WaitIdle
  Emit "G90"

  ProbeSide = t                  ' 트립 좌표 반환
End Function


'---------------------------------------------------------------------
' 안전장치 : 프로브가 실제로 신호를 내는지 "움직이기 전에" 확인한다.
'
' 종전에는 "이미 눌려 있는가" 만 보았다. 그래서 프로브를 연결하지 않아
' 입력이 영원히 0 인 경우를 그대로 통과시켰고, G31 은 트립을 받지 못한 채
' 탐색 거리를 끝까지 밀고 나갔다. 개발·시험 중에 장비를 연결하지 않는 것은
' 흔한 상황이므로, 그 상태를 매크로가 스스로 알아채고 멈춰야 한다.
'
' 확인은 매크로 실행당 한 번만 한다 (gProbeOK). 축마다 손을 대라고 하면
' 쓸 수 없는 도구가 된다.
' 반환 : 1 = 진행해도 좋음 / 0 = 중단 (gErr 이 함께 켜진다)
'---------------------------------------------------------------------
Function ProbeReady(ax)
  Dim i, seen
  ProbeReady = 0

  ' DRY_RUN 은 축을 움직이지 않으므로 확인할 것이 없다
  If DRY_RUN = 1 Then
    gProbeOK = 1
    ProbeReady = 1
    Exit Function
  End If

  ' (1) 이미 눌린 상태면 G31 이 즉시 트립되어 엉뚱한 좌표가 잡힌다
  If GetOemLed(PROBE_LED) <> 0 Then
    MsgBox "Probe input is ALREADY active on " & ax & "." & Chr(13) & "Check wiring / Active-Low setting, or move the tool off the probe." & Chr(13) & "Nothing has moved."
    gErr = 1
    Exit Function
  End If

  ' (2) 이번 실행에서 이미 확인했으면 넘어간다
  If gProbeOK = 1 Then
    ProbeReady = 1
    Exit Function
  End If

  ' (3) 확인을 끈 경우 (실기에서 반복 작업할 때만 0 으로 두십시오)
  If PROBE_CHECK <> 1 Then
    gProbeOK = 1
    ProbeReady = 1
    Exit Function
  End If

  ' (4) 손으로 눌러 신호가 실제로 들어오는지 본다. 15초 안에 반응이 없으면 중단.
  '     이 단계에서는 축을 전혀 움직이지 않는다.
  seen = 0
  i = 0
  Message "SAFETY : press the probe tip by hand now (15 s) - nothing will move"
  While i < 150 And seen = 0
    If GetOemLed(PROBE_LED) <> 0 Then
      seen = 1
    End If
    Sleep(100)
    i = i + 1
  Wend

  If seen = 0 Then
    MsgBox "Probe signal NOT detected." & Chr(13) & Chr(13) & "Nothing has moved - the macro stopped before any motion." & Chr(13) & "Check: probe wiring, Config > Ports and Pins > Input Signals > Probe (enabled / port / pin / Active Low)." & Chr(13) & "PROBE_LED is set to " & PROBE_LED & " - verify it matches your screen set." & Chr(13) & Chr(13) & "Set PROBE_CHECK = 0 only if you are certain the probe works."
    Message "aborted : probe signal not detected (nothing moved)"
    gErr = 1
    Exit Function
  End If

  ' (5) 손을 떼기를 기다린다. 눌린 채로 G31 을 걸면 0mm 에서 트립된다.
  i = 0
  Message "SAFETY : release the probe ..."
  While i < 150 And GetOemLed(PROBE_LED) <> 0
    Sleep(100)
    i = i + 1
  Wend
  If GetOemLed(PROBE_LED) <> 0 Then
    MsgBox "Probe is still triggered after 15 s." & Chr(13) & "Nothing has moved. Check the probe and try again."
    gErr = 1
    Exit Function
  End If

  gProbeOK = 1
  ProbeReady = 1
  Message "SAFETY : probe signal OK - probing starts"
End Function

'---------------------------------------------------------------------
' 한 축 2단계 프로빙 (빠른 탐색 -> 후퇴 -> 저속 정밀 측정)
'   i    : 축 번호 0=X 1=Y 2=Z
'   sgn  : 진행 방향 +1 또는 -1
'   dist : 최대 탐색 거리 [mm]
' 반환   : 현재 작업좌표계 기준 트립(접촉) 좌표
' 왜 두 번 재는가 : 빠른 속도로 재면 관성 때문에 부정확하므로,
' 일단 빠르게 찾고 -> 1mm 물러난 뒤 -> 아주 느리게 다시 재서
' 정밀도를 확보한다.
'---------------------------------------------------------------------
Function ProbeAxis(i, sgn, dist)
  Dim ax, t, k, acc, n           ' ax = 축 이름 문자열, t = 트립 좌표 임시 저장
  ax = AxisName(i)               ' 0/1/2 -> "X"/"Y"/"Z"
  ProbeAxis = 0                  ' 반환값을 일단 0 으로 초기화

  ' 안전장치 : 프로브가 실제로 신호를 내는지 확인한 뒤에만 움직인다
  If ProbeReady(ax) = 0 Then Exit Function

  ' 탐색 거리 상한 - 설정 실수로 축이 멀리 밀려나는 것을 막는다
  If dist > MAX_TRAVEL Then
    MsgBox "Search distance " & dist & " mm exceeds MAX_TRAVEL " & MAX_TRAVEL & " mm on " & ax & "." & Chr(13) & "Nothing has moved. Lower the setting or raise MAX_TRAVEL."
    gErr = 1
    Exit Function
  End If

  ' 1차 : 빠른 탐색 - G31(프로빙 이동)으로 접촉할 때까지 전진
  ' G91=상대좌표, F=이송속도. 예) 완성된 명령 = "G91 G31 X12 F200"
  Emit "G91 G31 " & ax & (sgn * dist) & " F" & FAST_F
  WaitIdle                     ' 이동 종료 대기
  Emit "G90"                     ' 절대좌표 모드 복귀

  ' 탐색 거리 안에서 접촉이 없었으면(LED 꺼진 채) 위치/배선 문제 -> 중단
  If DRY_RUN = 0 And GetOemLed(PROBE_LED) = 0 Then
    MsgBox "No contact within " & dist & " mm on " & ax & ". Aborted."
    gErr = 1
    Exit Function
  End If

  ' --- 저속 정밀 측정 : AVG_PASS 회 반복해 평균 ---
  ' 왜 여러 번 재는가 : 프로브는 같은 자리를 다시 재도 트리거 위치가 미세하게 흔들린다.
  ' N 번 재서 평균하면 그 흔들림(반복정밀도)이 대략 1/sqrt(N) 로 줄어든다.
  ' 다만 측정 시간이 N 배로 늘고, 선접촉(pretravel) 처럼 한쪽으로 치우친 오차는
  ' 평균해도 줄지 않는다. 그래서 2~3 회가 실용적인 상한이다.
  ' AVG_PASS = 1 이면 종전과 완전히 같은 동작이다.
  acc = 0
  n = 0
  For k = 1 To AVG_PASS
    ' 접촉면에서 RETRACT 만큼 후퇴 - 다음 저속 측정 준비
    Emit "G91 G0 " & ax & (-sgn * RETRACT)
    WaitIdle
    Emit "G90"

    ' 저속 정밀 프로빙 - 후퇴량의 2배 거리를 천천히 전진
    ' 최종 정확도는 이 저속 패스가 결정한다
    Emit "G91 G31 " & ax & (sgn * RETRACT * 2) & " F" & SLOW_F
    WaitIdle
    Emit "G90"

    ' 접촉을 놓쳤으면 중단 (진동/칩 오염 등 드문 경우)
    If DRY_RUN = 0 And GetOemLed(PROBE_LED) = 0 Then
      MsgBox "Slow pass " & k & " of " & AVG_PASS & " lost contact on " & ax & ". Aborted."
      gErr = 1
      Exit Function
    End If

    acc = acc + ReadTrip(i, sgn)       ' 접촉 순간의 좌표를 더해 둔다
    n = n + 1
  Next
  t = acc / n                    ' 평균이 이 축의 최종 측정값

  ' 면에서 1mm 떨어져 마무리 (스타일러스 보호)
  Emit "G91 G0 " & ax & (-sgn * RETRACT)
  WaitIdle
  Emit "G90"

  ProbeAxis = t                  ' 트립 좌표를 반환
End Function