'Tool Zero Setting Macro

' Change the following three lines to suit your machine
PlateThickness =0.5100	' Thickness of touch plate
RetractClearance = 0.100 ' Clearance above touch plate to retract to
ProbeFeed = 5		' Feedrate to use for probing

' Define some constants
ZDRO = 2		' Z Axis DRO
AbsoluteModeLED = 48	' Absolute Coordinate Mode LED
IncrementalModeLED = 49	' Incremental Coordinate Mode LED
FeedrateDRO = 818	' OEM code for feedrate DRO
ProbeLED = 825		' OEM code for probe input LED
ZTouchDRO = 2002	' OEM code for DRO that holds Z touch position

' Do nothing if probe is already grounded
If GetOemLed(ProbeLED) = 0 Then

   ' Wait a few seconds for user to get touchplate in place
   Code "G4 P2"
   
   ' Save current feedrate
   CurrentFeed = GetOemDRO(FeedrateDRO)
   
   ' Save current coordinate mode
   AbsMode = GetOemLED(AbsoluteModeLED)

   ' Set absolute coordinate mode
   Code "G90"

   ' Zero Z DRO
   Call SetDro (ZDRO, 0.0000)

   ' Pause for DRO to update
   Code "G4 P0.5"

   ' Do the touch move
   Code "G31 Z-0.5 F" & ProbeFeed

   ' Wait for it to complete
   While IsMoving()
   Wend

   ' Delay a bit for ???
   Code "G4 P0.25"

   ' Get the Z position where probe input triggered
   ZProbePos = GetVar(ZTouchDRO)

   ' Go back to touch position (G31 backs off after touch)
   Code "G0 Z" & ZProbePos

   ' Wait for it to complete
   While IsMoving ()
   Wend

   ' Set Z DRO to touch plate thickness
   Call SetDro (ZDRO, PlateThickness)

   ' Pause for DRO to update
   'Code "G4 P0.5"

   ' Retract Z to SafeZ, if enabled, else to RetractClearance above plate
   If(IsSafeZ() = 1) Then
      SafeZ = GetSafeZ
      If  SafeZ  > PlateThickness Then
         GotoSafeZ()
      End If
   Else
      RetractHeight = PlateThickness + RetractClearance
      Code "G0 Z" & RetractHeight
   End If
   
   ' Tell user we're done
   Code "(Z axis is now zeroed)"

   ' Reset feedrate to original value
   Code "F" & CurrentFeed
   
   ' Reset coordinate mode to original value
   If AbsMode = 0 Then
      Code "G91"
   End If
Else
   Code "(Z-Plate is grounded, check connection and try again)"
End If 

Exit Sub