Manual tool change + tool lengh touch off

[MACROS]
MACRO = init_probe

[TOUCHPLATE]
# Touchplate dimensions
OFFSETX = 3.306
OFFSETY = 3.767
OFFSETZ = 6.172

[TOOLSENSOR]
# Absolute coordinates of the toolsetter pad
X = 680.5
Y = 6.0
# Absolute Z start search coordinates, adjust to match the longest tool
Z =-25.0
# Absolute Z height to safely travel to toolsetter
TRAVELZ = 0.0
# Maximum search distance and direction (sign), adjust to match the difference between the longest and shortest tools
MAXPROBE = -25

[CHANGE_POSITION]
# Absolute coordinates of the tool change point
X = 650.0
Y = 100.0
Z = -10.0

[RS274NGC]
# Relative path to macros folder
SUBROUTINE_PATH = macros
REMAP=M6 modalgroup=6 prolog=change_prolog ngc=change epilog=change_epilog

O<change> sub
  ;(debug, in change tool_in_spindle=#<tool_in_spindle> current_pocket=#<current_pocket>;)
  ;(debug, selected_tool=#<selected_tool> selected_pocket=#<selected_pocket>;)
  ;(debug, current tool z_offset=#5403)
 
  ;otherwise after the M6 this information is gone!
  #<xpos> = #<_x>                              (PRINT,save absolute X=#<_x>;)
  #<ypos> = #<_y>                              (PRINT,save absolute Y=#<_y>;)
  #<tlo> = #5403                               (PRINT,current offset for tool #<_selected_tool>=#<tlo>;)
  #<cht> = #<_ini[CHANGE_POSITION]TRAVELZ>
  #<chx> = #<_ini[CHANGE_POSITION]X>
  #<chy> = #<_ini[CHANGE_POSITION]Y>
  #<chz> = #<_ini[CHANGE_POSITION]Z>
  #<tsm> = #<_ini[TOOLSENSOR]MAXPROBE>
  #<tsx> = #<_ini[TOOLSENSOR]X>
  #<tsy> = #<_ini[TOOLSENSOR]Y>
  #<tsz> = #<_ini[TOOLSENSOR]Z>

  ; we must execute this only in the milltask interpreter
  ; or preview will break, so test for '#<_task>' which is 1 for
  ; the milltask interpreter and 0 in the UI's
  O100 if [#<_task> EQ 0]
  O100 return [999]
  O100 endif
 
  O200 if [#<_hal[gmoccapy.toolmeasurement]> EQ 0]
  O200 return [3] ; indicate no tool measurement
  O200 endif
 
  O300 if [#<_hal[gmoccapy.searchvel]> LE 0]
  O300 return [-1] ; indicate searchvel <= 0
  O300 endif
 
  O400 if [#<_hal[gmoccapy.probevel]> LE 0]
  O400 return [-2] ; indicate probevel <= 0
  O400 endif
 
  G53 G0 Z[#<cht>]                             (PRINT,move to travel height)
 
 
  G53 G0 X[#<chx>] Y[#<chy>]                   (PRINT,move to change position)
  G53 G0 Z[#<chz>]
  G49                                          (PRINT,cancel tool offset)
  G92.2                                        (PRINT,turn off G92 offsets)
 
  M6                                           (PRINT,real change tool)
 
  O700 if [#<_selected_tool> EQ 99]
    G10 L1 P99 Z0                              (PRINT,reset probe offset)
  O700 else
    G53 G0 X[#<tsx>] Y[#<tsy>]                 (PRINT,move to toolsensor)
    G53 G0 Z[#<tsz>]
    
    F #<_hal[gmoccapy.searchvel]>              (PRINT,set search speed)
    G91                                        (PRINT,relative move )
    G38.2 Z#<tsm>                              (PRINT,touch sensor)
    G0 Z2                                      (PRINT,release sensor)
    F #<_hal[gmoccapy.probevel]>               (PRINT,set probe speed)
    G38.2 Z-4                                  (PRINT,touch sensor)
    G90                                        (PRINT,absolute move)
    
    O800 if [#5070 EQ 0]
    O800 return [-3] ; indicate probe contact failure to epilog
    O800 endif
    
    G10 L10 P[#<_selected_tool>] Z[#4963]      (PRINT,set absolute offset for tool #<_selected_tool>;)
    G43
  O700 endif
  G92.3                                        (PRINT,restore G92 offsets)
  G53 G0 Z[#<cht>]                             (PRINT,move to travel height)
  G0 X[#<xpos>] Y[#<ypos>]                     (PRINT,return to where we were in WCS X=#<xpos> Y=#<ypos>;)
 
; signal success be returning a value > 0:
O<change> endsub [1]
M2

O<init_probe> SUB
                                                                   (PRINT,Probe init)
  G94                                                              (PRINT,use feedrate in units/min )
  G92.1                                                            (PRINT,remove G92 offsets)
  G90                                                              (PRINT,absolute distance mode)
  G49                                                              (PRINT,cancel tool length compensation)
  G40                                                              (PRINT,turn cutter compensation off)
  G10 L2 P0 X0 Y0 Z0                                               (PRINT,reset coordinate system)
  O100 IF [ #<_current_tool> NE 99 ]                               (PRINT,tool loaded: #<_current_tool>;)
    T99 M6                                                         (PRINT,mount probe)
  O100 ENDIF
  O101 IF [ #<_metric> EQ 1 ]                                      (PRINT,set probe feeds according to units )
    #<fast> =  150.0                                               (PRINT,selected metric units )
    #<slow> =   30.0
    #<retract> = 3.0
    #<tool_diam> = #5410                                           (PRINT,tool diameter=#<tool_diam>mm)
  O101 ELSE
    #<fast> =    6.0                                               (PRINT,selected imperial units )
    #<slow> =    1.2
    #<retract> = 0.1
    #<tool_diam> = #5410                                           (PRINT,tool diameter=#<tool_diam>in)
  O101 ENDIF
                                                                   (PRINT,touch plate Z probe)
  G38.2 Z[#<_ini[AXIS_Z]MIN_LIMIT>] F[#<fast>]                     (PRINT,Z probe toward workpiece with error )
  G91                                                              (PRINT,relative move )
  G0 Z[#<retract>]                                                 (PRINT,retract)
  G0 Z-[#<retract>/2]                                              (PRINT,move forward half retract distance)
  G38.2 Z-[#<retract>] F[#<slow>]                                  (PRINT,Z probe slowly, got Z probe at #5063)
  G10 L20 P0 Z[#<_ini[TOUCHPLATE]OFFSETZ>]                         (PRINT,set Z offset in current WCS)
  G0 Z#<retract>                                                   (PRINT,retract)
   
                                                                   (PRINT,touch plate X probe)
  G0 X-[#<retract> + #<tool_diam>]                                 (PRINT,move in front of touch plate)
  G0 Z-[#<retract> * 2]                                            (PRINT,go down by twice retract length)
  G38.2 X[#<retract> + #<tool_diam>] F[#<fast>]                    (PRINT,X probe toward workpiece with error )
  G0 X-[#<retract>]                                                (PRINT,retract)
  G0 X[#<retract>/2]                                               (PRINT,move forward half retract distance)
  G38.2 X[#<retract> + #<tool_diam>] F[#<slow>]                    (PRINT,X probe slowly, got X probe at #5061)
  G10 L20 P0 X-[#<_ini[TOUCHPLATE]OFFSETX> + #<tool_diam> / 2]     (PRINT,set X offset in current WCS)
  G0 X-[#<retract>] Z[#<retract>*2]                                (PRINT,retract)
 
                                                                   (PRINT,Touch plate Y probe)
  G0 X[#<retract> + #<tool_diam>] Y-[#<retract> + #<tool_diam>]    (PRINT,move below of touch plate)
  G0 Z-[#<retract>*2]                                              (PRINT,go down by twice retract length)
  G38.2 Y[#<retract> + #<tool_diam>] F[#<fast>]                    (PRINT,X probe toward workpiece with error)
  G0 Y-[#<retract>]                                                (PRINT,retract)
  G0 Y[#<retract>/2]                                               (PRINT,move forward half retract distance)
  G38.2 Y[#<retract> + #<tool_diam>] F[#<slow>]                    (PRINT,Y probe slowly, got Y probe at #5062)
  G10 L20 P0 Y-[#<_ini[TOUCHPLATE]OFFSETY> + #<tool_diam> / 2]     (PRINT,set Y offset in current WCS)
  G0 Y-[#<retract>] Z[#<retract> * 2]                              (PRINT,retract)

  G90                                                              (PRINT,absolute move)

  G0 X0 Y0                                                         (PRINT,move to travel height)
  G53 G0 Z[#<_ini[CHANGE_POSITION]TRAVELZ>]                        (PRINT,move to safe travel height)
  G53 G0 X[#<_ini[TOOLSENSOR]X>] Y[#<_ini[TOOLSENSOR]Y>]           (PRINT,move to toolsensor)
  G53 G0 Z[#<_ini[TOOLSENSOR]Z>]
 
  F #<_hal[gmoccapy.searchvel]>                                    (PRINT,set search speed)
  G91                                                              (PRINT,relative move )
  G38.2 Z#<_ini[TOOLSENSOR]MAXPROBE>                               (PRINT,touch sensor)
  G0 Z2                                                            (PRINT,release sensor)
  F #<_hal[gmoccapy.probevel]>                                     (PRINT,set probe speed)
  G38.2 Z-4                                                        (PRINT,touch sensor)
  #4963 = #5063                                                    (PRINT,store probe trigger Z value)
  G90                                                              (PRINT,absolute move)
  G53 G0 Z[#<_ini[CHANGE_POSITION]TRAVELZ>]                        (PRINT,move to travel height)
  G53 G0 X[#<_ini[CHANGE_POSITION]X>] Y[#<_ini[CHANGE_POSITION]Y>] (PRINT,move to change position)
  G53 G0 Z[#<_ini[CHANGE_POSITION]Z>]

O<init_probe> ENDSUB
M2