manual tool change with tool length probe

O<tool-change> SUB
(file tool-change.ngc)

(--- header ---)
(tuxcnc 2021)
(manual tool change with automatic measurement)
(replaces M6 g-code)
(we assume that, toolsensor is permanently mounted)
(and Z parameters of TOOL_CHANGE_POSITION and TOOLSENSOR ZPOS)
(are maximum upper, otherwise you may get collision)
(you must have the following in your INI file)
(without semicolons and you can change parameters)
;[RS274NGC]
;FEATURES = 4
;REMAP=M6 modalgroup=6 ngc=tool-change
;[EMCIO]
;TOOL_CHANGE_QUILL_UP = 1
;TOOL_CHANGE_POSITION = 230 0 0
;[TOOLSENSOR]
;XPOS = 192
;YPOS = 0
;ZPOS = 0
;APOS = 0
;BPOS = 0
;CPOS = 0
;UPOS = 0
;VPOS = 0
;WPOS = 0
;FEED1 = 500
;FEED2 = 100
;TRAVEL =100
;DISTANCE = 71
(--- end of header ---)

#<tool> = #<_selected_tool>
#<previous_tool> = #<_current_tool>

O100 if [#<_task> EQ 0]    ( only run the subroutine in milltask interpreter )
        (debug, Task ist Null)
O100     return [999]
O100 endif

o200 IF [ #<tool> EQ 0 ]    ( check if tool zero selected and end routine )
(MSG, No Tool Selected )
O200     return [999]
o200 ENDIF

(---  store existing parameters  ---)
#<g30x> = #5181
#<g30y> = #5182
#<g30z> = #5183
#<g30a> = #5184
#<g30b> = #5185
#<g30c> = #5186
#<g30u> = #5187
#<g30v> = #5188
#<g30w> = #5189
#<absolute> = #<_absolute>  (remember in local variable if G90 was set) 
#<mist> = #<_mist>         (remember in local variable if M7 was set)
#<feed> = #<_feed>         (remember feed value)
#<flood> = #<_flood>        (remember in local variable if M8 was set)
#<tool_offset> = #<_tool_offset>

G49             ( cancel tool length compensation )
M9             ( coolant off )
M6                 ( run normal M6 operation )
G91             ( incremental distance mode )
(---   prepare g30     ----)
#5181 = #<_ini[TOOLSENSOR]XPOS>
#5182 = #<_ini[TOOLSENSOR]YPOS>
#5183 = #<_ini[TOOLSENSOR]ZPOS>
#5184 = #<_ini[TOOLSENSOR]APOS>
#5185 = #<_ini[TOOLSENSOR]BPOS>
#5186 = #<_ini[TOOLSENSOR]CPOS>
#5187 = #<_ini[TOOLSENSOR]UPOS>
#5188 = #<_ini[TOOLSENSOR]VPOS> 
#5189 = #<_ini[TOOLSENSOR]WPOS>
(--------------------------)                        
G30 ( go to toolsensor )
#<PlaneOffset> = [#5183 - #5422]  ( get the current plane Z offset )    
G38.2 Z-[#<_ini[TOOLSENSOR]TRAVEL>] F[#<_ini[TOOLSENSOR]FEED1>]  ( fast touch )
G38.4 Z1 F[#<_ini[TOOLSENSOR]FEED2>]
G10 L1 P#<tool> Z-[-#<PlaneOffset> - #5063 - #<_ini[TOOLSENSOR]DISTANCE>] ( store offset in tooltable )
G30 ( go to safe position )
#<previous_tool> = #<tool>

(--- restore parameters ---)
#5181 = #<g30x>
#5182 = #<g30y>
#5183 = #<g30z>
#5184 = #<g30a>
#5185 = #<g30b>
#5186 = #<g30c>
#5187 = #<g30u>
#5188 = #<g30v> 
#5189 = #<g30w>
O<restore_abs> if [#<absolute>]
    G90 (restore only if it was set on entry:)
O<restore_abs> endif
O<restore_mist> if [#<mist>]
    M7 (restore only if it was set on entry:)
O<restore_mist> endif
O<restore_flood> if [#<flood>]
    M8 (restore only if it was set on entry:)
O<restore_flood> endif
O<restore_tool_offset> if [#<tool_offset>]
    G43 (restore only if it was set on entry:)
O<restore_tool_offset> endif
F #<feed> (restore previous feed value)
(--------------------------)

O<tool-change> ENDSUB
M2