o<aaa> sub
(DEBUG, from tool_in_spindle=#<tool_in_spindle> to selected_tool=#<selected_tool>)

; -------------------------------------
; --- Begin Initial data gathering ----
; -------------------------------------

#<fast_probe_fr> = #3004    (set from probe screen fast probe feed rate)
#<slow_probe_fr> = #3005    (set from probe screen slow probe feedrate)
#<traverse_fr> = #3006    (set from probe screen traverse probe feedrate)
#<z_max_travel> = #3007    (max z distance the tool travels before erroring out if not contact is made)
#<xy_max_travel> = #3008    (max xy distance the tool travels before erroring out if not contact is made)
#<retract_distance> = #3009    (distance the tool retracts after making contact during fast feed mode)
#<spindle_zero_height> = #3010    (G53 distance from home to spindle nose triggering point on touch plate)
#<tool_diameter_probe_mode> = #3011    (activates the tool diameter probe subroutine section)
#<tool_diameter_offset_mode> = #3012    (activates the tool diameter offset position for probe subroutine section)
#<tool_setter_offset_direction> = #3013    (sets tool setter offset direction to move tool)
#<tool_diameter> = #5410    (current tool's diameter used for offseting probe position in x axis)

G92.1    (Cancel G92 offset)
M50 P0

#<tool_touch_x_coords> = #5181
#<tool_touch_y_coords> = #5182
#<tool_touch_z_coords> = #5183

#<tool_change_x_coords> = 1
#<tool_change_y_coords> = -16
#<tool_change_z_coords> = 0


#<tool_radius_offset> = [#<tool_diameter> / 2]
#<left_offset_probing_position> = [#<tool_touch_x_coords> - #<tool_radius_offset>]
#<right_offset_probing_position> = [#<tool_touch_x_coords> + #<tool_radius_offset>]
#<front_offset_probing_position> = [#<tool_touch_y_coords> - #<tool_radius_offset>]
#<back_offset_probing_position> = [#<tool_touch_y_coords> + #<tool_radius_offset>]

#<tool> = #<selected_tool>
#<pocket> = #<selected_pocket>
#<local_start_x> = #<_x>
#<local_start_y> = #<_y>
#<local_start_z> = #<_z>

#<zworkoffset> = [#[5203 + #5220 * 20] + #5213 * #5210]
;#<offset_z> = #5422         ;Stores the offset of the current Z coordinate.

o100 if [#<tool_diameter_offset_mode> EQ 1]
    o101 if [#<tool_setter_offset_direction> EQ 0]
        #<tool_touch_x_coords> = #<left_offset_probing_position>
    o101 else if [#<tool_setter_offset_direction> EQ 1]
        #<tool_touch_x_coords> = #<right_offset_probing_position>
    o101 else if [#<tool_setter_offset_direction> EQ 2]
        #<tool_touch_y_coords> = #<front_offset_probing_position>
    o101 else if [#<tool_setter_offset_direction> EQ 3]
        #<tool_touch_y_coords> = #<back_offset_probing_position>
    o101 endif
o100 endif

; -------------------------------------
; --- End Initial Data Gathering ----
; -------------------------------------

; -------------------------------------
; --- Begin Initial Safety Checks ----
; -------------------------------------

; 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
o110 if [#<_task> EQ 0]
        (debug, Task is Null)
o110     return [999]
o110 endif

; check we are in right mode
o120 if [#<_metric_machine>]
    o125 if [#<_imperial>]
       (MSG, Auto Tool probe error: not in G21 mode )
    o125 return [-3] ; indicate probe contact failure to epilog
    o125 endif
o120 else
    o125 if [#<_imperial> EQ 0]
       (MSG, Auto Tool probe error: not in G20 mode )
    o125 return [-3] ; indicate probe contact failure to epilog
    o125 endif
o120 endif

; -------------------------------------
; --- End Initial Safety Checks ----
; -------------------------------------

; -------------------------------------
; --- Begin Physical Tool Change ----
; -------------------------------------

G90    (set absolute coordinates)
G53 G0 F[#<traverse_fr>] Z0    (move to z0 home position)

o200 if [#<_current_tool>  NE #<tool>]
    ; then move to change position
    G53 G0 F[#<traverse_fr>] X#<tool_change_x_coords> Y#<tool_change_y_coords>
    G53 G0 F[#<traverse_fr>] Z#<tool_change_z_coords>
o200 endif

;cancel tool offset mode
G49

; using the code being remapped here means 'use builtin behaviour'
;  ie, this is not a recursive call to this program, but calls the 
;    un-remapped M6 processing 
;  That processing includes actually sending the window to the user
;    to press OK when they have completed the tool change.
M6

; -------------------------------------
; ------ End Physical Tool Change ----
; -------------------------------------

; -------------------------------------
; ------ Begin Safety Checks for Tool Offset Measurement ----
; -------------------------------------


; -------------------------------------
; ------ Begin Tool Offset Measurement ----
; -------------------------------------

G90    (set absolute coordinates)
G53 G1 F[#<traverse_fr>] Z0    (move to z0 home position)
G53 G1 F[#<traverse_fr>] X#<tool_touch_x_coords> Y#<tool_touch_y_coords>
G53 G1 F[#<traverse_fr>] Z#<tool_touch_z_coords>

#<offset_z> = #5422         ;Stores the offset of the current Z coordinate.

;switch to relative distance mode for probe moves
G91

F #<fast_probe_fr>
G38.2 Z-[#<z_max_travel>]    (fast tool probe)
#<z_fast_probe> = #5063    (save probe result of fast probe to parameters)
G1 F[#<traverse_fr>] Z[#<retract_distance>]    (retract tool retract distance amount)

o400 if [#5070 EQ 0]
    G90
o400 return [-3] ; indicate probe contact failure to epilog
o400 endif

(Slow Probe Rule, if Slow Probe FR is set to 0, Slow Probe is Bypassed)
o410 if [#<slow_probe_fr> GT 0]
    (Initiate Slow Z- Probe)
    G91
    F[#<slow_probe_fr>]    (set probe slow feedrate)
    G38.2 Z-[#<retract_distance> * 2]    (slow tool probe)
    #<z_slow_probe> = #5063
o410 endif

o420 if [#5070 EQ 0]
    G90
o420 return [-3] ; indicate probe contact failure to epilog
o420 endif

(Tool Diameter Probe Mode Section, User must define this section as needed)
o430 if [#<tool_diameter_probe_mode> EQ 1]
    (DEBUG, Tool Diameter Probing is Not Defined in Subroutine)
o430 endif



G90    (set absolute coordinates)
G53 G1 F[#<traverse_fr>] Z0 (Send Spindle to home zero position)

(define new tool length offset parameters)
#<new_tool_length_offset> = [ABS[#<spindle_zero_height> + #5063 - #<offset_z>]]

G10 L1 P #<tool> Z [#<new_tool_length_offset>]

T #<tool> G43  H #<tool>    (enable tool length offset)

M50 P1  (reinstate feedrate override)
(DEBUG, new tool lenght offset: #<new_tool_length_offset>)
(DEBUG, offset_z: #<offset_z>, spindle zero offset: #<spindle_zero_height>, fast probe: #<z_fast_probe>, slow probe: #<z_slow_probe>)

o<aaa> endsub [1]
M2