diff --git a/configs/atc_sim/macros_sim_inch/clamptool.ngc b/configs/atc_sim/macros_sim_inch/clamptool.ngc
index a167d778..8b1a7b1b 100644
--- a/configs/atc_sim/macros_sim_inch/clamptool.ngc
+++ b/configs/atc_sim/macros_sim_inch/clamptool.ngc
@@ -4,7 +4,7 @@ M65 P2 ; clamp the tool
 
 M66 P5 L3 Q2 ; check the clamped tool sensor
 o100 if [#5399 LT 0]
-    (abort, failed to release tool) ; abort if the sensor does not activate in 2 seconds
+    (abort, Failed to release tool) ; abort if the sensor does not activate in 2 seconds
 o100 endif
 
 o<clamptool> endsub [1]
diff --git a/configs/atc_sim/macros_sim_inch/extendatc.ngc b/configs/atc_sim/macros_sim_inch/extendatc.ngc
index b16d6413..95828367 100644
--- a/configs/atc_sim/macros_sim_inch/extendatc.ngc
+++ b/configs/atc_sim/macros_sim_inch/extendatc.ngc
@@ -20,7 +20,7 @@ M64 P0 ; Move Carousel OUT
 M66 P1 L3 Q5 ; check for carousel out position sensor
 o100 if [#5399 LT 0]
     M65 P0 ; switch off atc out solenoid
-    (abort, atc not in position)
+    (abort, ATC not in position)
 o100 endif
 
 o<extendatc> endsub [1]
diff --git a/configs/atc_sim/macros_sim_inch/m10.ngc b/configs/atc_sim/macros_sim_inch/m10.ngc
index c02e0788..2562672e 100644
--- a/configs/atc_sim/macros_sim_inch/m10.ngc
+++ b/configs/atc_sim/macros_sim_inch/m10.ngc
@@ -4,7 +4,7 @@ o<m10> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M10> P#<p>)
+(PRINT, o<m10> P#<p>)
 
 o100 if [#3989 NE 1]
     M13
@@ -17,7 +17,7 @@ o101 if [EXISTS[#<_ini[atc]pockets>]]
 o101 endif
 
 #<steps> = [#3990 - #<p>]
-(PRINT, o<M10> P#<p>, steps=#<steps>)
+(PRINT, o<m10> P#<p>, steps=#<steps>)
 o110 if [#<steps> GT [#<number_of_pockets> / 2]]
     #<steps>=[#<steps> - #<number_of_pockets>]
 o110 endif
@@ -31,6 +31,7 @@ o130 elseif [#<steps> LT 0]
     M11 P[#<steps>]
 o130 endif
 
+(PRINT, o<m10> endsub)
 o<m10> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_inch/m11.ngc b/configs/atc_sim/macros_sim_inch/m11.ngc
index 096f578b..3db74563 100644
--- a/configs/atc_sim/macros_sim_inch/m11.ngc
+++ b/configs/atc_sim/macros_sim_inch/m11.ngc
@@ -5,7 +5,7 @@ o<m11> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M11> P#<p>)
+(PRINT, o<m11> P#<p>)
 
 o100 if [#3989 NE 1]
     (PRINT, atc not homed, homing)
@@ -34,7 +34,7 @@ o120 do
     M66 P4 L1 Q3 ; wait for rising edge on rotation index
     o130 if [#5399 LT 0]
         M65 P4 ; Stop atc motor
-        (abort, failed to get rotation index)
+        (abort, Failed to get rotation index)
     o130 endif
     #3990 = [[[#3990+2] MOD #<number_of_pockets>]-1] ; Pocket is no.1-#<number_of_pockets>
     #<steps_to_move> = [#<steps_to_move>-1]
@@ -44,6 +44,7 @@ M65 P4 ; Stop motor
 
 #<_my_current_pocket> = #3990
 
+(PRINT, o<m11> endsub)
 o<m11> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_inch/m12.ngc b/configs/atc_sim/macros_sim_inch/m12.ngc
index c1d4c3ca..34f2d6f3 100644
--- a/configs/atc_sim/macros_sim_inch/m12.ngc
+++ b/configs/atc_sim/macros_sim_inch/m12.ngc
@@ -5,7 +5,7 @@ o<m12> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M12> P#<p>)
+(PRINT, o<m12> P#<p>)
 
 o100 if [#3989 NE 1]
     (PRINT, atc not homed, homing)
@@ -34,7 +34,7 @@ o120 do
     M66 P4 L1 Q3 ; wait for rising edge on rotation index
     o130 if [#5399 LT 0]
         M65 P3 ; Stop atc motor
-        (abort, failed to get rotation index)
+        (abort, Failed to get rotation index)
     o130 endif
     #3990 = [[[#3990-2] MOD #<number_of_pockets>]+1] ; Pocket is no.1-#<number_of_pockets>
     #<steps_to_move> = [#<steps_to_move>-1]
@@ -44,6 +44,7 @@ M65 P3 ; Stop motor
 
 #<_my_current_pocket> = #3990
 
+(PRINT, o<m12> endsub)
 o<m12> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_inch/m13.ngc b/configs/atc_sim/macros_sim_inch/m13.ngc
index 06da95e4..ede03334 100644
--- a/configs/atc_sim/macros_sim_inch/m13.ngc
+++ b/configs/atc_sim/macros_sim_inch/m13.ngc
@@ -4,7 +4,7 @@ o<m13> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M13>)
+(PRINT, o<m13>)
 
 (DEBUG, EVAL[vcp.getWidget{"dynatc"}.atc_message{"REFERENCING"}])
 
@@ -12,7 +12,7 @@ M64 P3 ; Move Motor FWD
 M66 P3 L1 Q20 ; wait for rising edge on home index
 o100 if [#5399 LT 0]
     M65 P3 ; stop motor
-    (abort, failed to home carousel)
+    (abort, Failed to home carousel)
 o100 endif
 
 #3990 = 1
@@ -36,6 +36,7 @@ o120 endwhile
 
 M61 Q#3991 G43 H#3991 #5210 = 0
 
+(PRINT, o<m13> endsub)
 o<m13> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_inch/m21.ngc b/configs/atc_sim/macros_sim_inch/m21.ngc
index 9bffddd1..2ec9daea 100644
--- a/configs/atc_sim/macros_sim_inch/m21.ngc
+++ b/configs/atc_sim/macros_sim_inch/m21.ngc
@@ -5,7 +5,7 @@ o<m21> sub
 ; Parameter #3991 is used to track the current tool loaded it in the spindle (persistently)
 ; #<atc_z_tool_change_height> is the height you spindle needs to be at to clamp/unclamp a tool form the ATC (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
 ; #<atc_z_tool_clearance_height> is the clearance height you spindle needs to be at to safely clear the ATC (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)
-(PRINT, o<M21>)
+(PRINT, o<m21>)
 
 #<atc_z_tool_change_height> = -3.9000
 o101 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
@@ -29,7 +29,7 @@ M64 P0 ; Move Carousel out
 M66 P1 L3 Q5 ; check for carousel out sensor
 o100 if [#5399 LT 0]
     M65 P0 ; switch off atc out solenoid
-    (abort, atc not in position)
+    (abort, ATC not in position)
 o100 endif
 
 M24 ; activate drawbar, release the tool
@@ -39,6 +39,7 @@ G0 G53 Z#<atc_z_tool_clearance_height> ; move z to clear height
 
 #3991 = 0; save fact there is now no tool in the spindle
 
+(PRINT, o<m21> endsub)
 o<m21> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_inch/m22.ngc b/configs/atc_sim/macros_sim_inch/m22.ngc
index 825713fa..2e87c9a5 100644
--- a/configs/atc_sim/macros_sim_inch/m22.ngc
+++ b/configs/atc_sim/macros_sim_inch/m22.ngc
@@ -4,7 +4,7 @@ o<m22> sub
 ; after loading any tool in the current pocket to the spindle
 ; #<atc_z_tool_change_height> is the height you spindle needs to be at to clamp/unclamp a tool form the ATC (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
 ; #<atc_z_tool_clearance_height> is the clearance height you spindle needs to be at to safely clear the ATC (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)
-(PRINT, o<M22>)
+(PRINT, o<m22>)
 
 #<atc_z_tool_change_height> = -3.9000
 o101 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
@@ -24,18 +24,19 @@ M65 P2 ; release the drawbar to clamp the tool
 M5
 M66 P5 L3 Q1 ; check the tool clamped sensor
 o100 if [#5399 LT 0]
-    (abort, failed to reclamp tool)
+    (abort, Failed to reclamp tool)
 o100 endif
 
 M65 P0 ; Move Carousel home
 M66 P0 L3 Q4 ; check carousel in position sensor
 o110 if [#5399 LT 0]
     M65 P1 ; turn off the solenoid to send atc home
-    (abort, failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
+    (abort, Failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
 o110 endif
 
 ;M65 P1
 
+(PRINT, o<m22> endsub)
 o<m22> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_inch/m24.ngc b/configs/atc_sim/macros_sim_inch/m24.ngc
index 484fc11b..37454968 100644
--- a/configs/atc_sim/macros_sim_inch/m24.ngc
+++ b/configs/atc_sim/macros_sim_inch/m24.ngc
@@ -8,6 +8,7 @@ o100 if [#5399 LT 0]
     (abort, failed to release tool) ; abort if the sensor does not activate in 3 seconds
 o100 endif
 
+(PRINT, o<m24> endsub)
 o<m24> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_inch/m25.ngc b/configs/atc_sim/macros_sim_inch/m25.ngc
index dc55c0f5..a1a7ac98 100644
--- a/configs/atc_sim/macros_sim_inch/m25.ngc
+++ b/configs/atc_sim/macros_sim_inch/m25.ngc
@@ -6,9 +6,10 @@ M64 P0 ; Move Carousel out
 M66 P1 L3 Q5 ; check for carousel out sensor
 o100 if [#5399 LT 0]
     M65 P0 ; switch off atc out solenoid
-    (abort, atc not in position)
+    (abort, ATC not in position)
 o100 endif
 
+(PRINT, o<m25> endsub)
 o<m25> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_inch/retractatc.ngc b/configs/atc_sim/macros_sim_inch/retractatc.ngc
index 43c31da6..f9d400fe 100644
--- a/configs/atc_sim/macros_sim_inch/retractatc.ngc
+++ b/configs/atc_sim/macros_sim_inch/retractatc.ngc
@@ -6,7 +6,7 @@ M64 P1 ; Move Carousel IN
 M66 P0 L3 Q5 ; check carousel in position sensor
 o100 if [#5399 LT 0]
     M65 P1 ; turn off the solenoid to send atc home
-    (abort, failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
+    (abort, Failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
 o100 endif
 M65 P1
 
diff --git a/configs/atc_sim/macros_sim_inch/toolchange.ngc b/configs/atc_sim/macros_sim_inch/toolchange.ngc
index de63b409..21d39094 100644
--- a/configs/atc_sim/macros_sim_inch/toolchange.ngc
+++ b/configs/atc_sim/macros_sim_inch/toolchange.ngc
@@ -6,8 +6,10 @@ o<toolchange> sub
 ; Parameters #4001 to #4024 are used to track which tool is in which pocket (persistently)
 ; Parameter #4000 is not populated just used in the maths to calculate the above numbers
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
+; #<atc_z_tool_change_height> is the height you spindle needs to be at to clamp/unclamp a tool form the ATC (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
+; #<atc_z_tool_clearance_height> is the clearance height you spindle needs to be at to safely clear the ATC (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)
 
-(PRINT, o<toolchange>)
+(PRINT, o<toolchange> selected_tool: #<selected_tool>, tool_in_spindle: #<tool_in_spindle>, selected_pocket: #<selected_pocket>, current_pocket: #<current_pocket>, task: #<_task>)
 
 o100 if [#<_task> EQ 0]
     (DEBUG, Task is null)
@@ -20,28 +22,46 @@ o101 if [EXISTS[#<_ini[atc]pockets>]]
     #<number_of_pockets> = #<_ini[atc]pockets>
 o101 endif
 
+#<atc_z_tool_change_height> = -3.9000
+o102 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
+    #<atc_z_tool_change_height> = #<_ini[atc]z_tool_change_height>
+o102 endif
+#<atc_z_tool_clearance_height> = [#<_ini[AXIS_Z]MAX_LIMIT>-0.1]
+o103 if [EXISTS[#<_ini[atc]z_tool_clearance_height>]]
+    #<atc_z_tool_clearance_height> = #<_ini[atc]z_tool_clearance_height>
+o103 endif
+
 ; assign the programmable coolant parameters
 #<activate_programmable_coolant> = #1 (=0)
 #<horizontal_spindle_nozzle_dist> = #2 (=0)
 #<vertical_spindle_nozzle_dist> = #3 (=0)
 #<pc_angle_offset> = #4 (=0)
 
-; assign the variables passed by M6 to some parameters
+; assign the variables passed by M6 change_prolog to some parameters
 #100 = #<selected_tool>
 #110 = #<tool_in_spindle>
 #120 = #<selected_pocket>
+#121 = #<current_pocket>
+; NOTE:
+;      The legacy names *selected_pocket* and *current_pocket* actually reference
+;      a sequential tooldata index for tool items loaded from a tool
+;      table ([EMCIO]TOOL_TABLE) or via a tooldata database ([EMCIO]DB_PROGRAM)
 
-o110 if [#100 EQ #110] ; checks if tool in the spindle is same as requested
+o110 if [#<selected_tool> EQ #<tool_in_spindle>] ; checks if tool in the spindle is same as requested
     o<toolchange> endsub [1]
     M2
 o110 endif
 
+o111 if [#3991 NE #<tool_in_spindle>]
+    (PRINT, o<toolchange> tool_in_spindle does not match 3991)
+o111 endif
+
 #<next_pocket> = 0 ; assigns 0 to the next pocket for a later check if the tool is found in the carousel
 #<open_pocket> = 0
 #130 = #<number_of_pockets> ; assign test parameter the number of pockets in the carousel
 
 o120 do
-    o121 if [#[4000 + #130] EQ #100] ; checks all pockets to see if it contains tool number requested as the new tool
+    o121 if [#[4000 + #130] EQ #<selected_tool>] ; checks all pockets to see if it contains tool number requested as the new tool
         #<next_pocket> = #130 ; if tool is found in pocket, assigns the next pocket
     o121 endif
     o122 if [#[4000 + #130] EQ 0] ; checks if the pocket is empty, last pocket checked will be the lowest empty pocket number, for putting tool in spindle away.
@@ -50,7 +70,7 @@ o120 do
     #130 = [#130 - 1]
 o120 while [#130 GT 0]
 o130 if [#<next_pocket> EQ 0] ; if tool is not found, aborts and sends a message
-    (abort, tool not in carousel)
+    (abort, Tool T%d#<selected_tool> not found in carousel)
 o130 endif
 
 ; now we know which pocket the next tool is sitting in
@@ -59,7 +79,7 @@ o130 endif
 
 o140 if [#<tool_in_spindle> GT 0] ; checks if there is a valid tool in the spindle
     o141 if [#<open_pocket> EQ 0] ; If there is a tool in the spindle, checks if there is an open pocket
-        (abort, carousel is full, cant put away tool in into carousel)
+        (abort, Carousel is full, cant put away tool T#<tool_in_spindle> in into carousel)
     o141 endif
     M10 P[#<open_pocket>] ; move carousel to an open pocket
     M21 ; puts the tool in spindle away into the open pocket
@@ -67,10 +87,12 @@ o140 if [#<tool_in_spindle> GT 0] ; checks if there is a valid tool in the spind
     #140 = #<open_pocket>
     #[4000 + #140] = #<tool_in_spindle> ; save tool number in pocket
     #3991 = 0 ; empty tool in the spindle
+    M61 Q0
+    G49
 o140 endif
 
 G90
-G0 G53 Z0
+G0 G53 Z#<atc_z_tool_clearance_height> ; move z to clear height
 
 o150 if [#<selected_tool> GT 0] ; selected tool is not tool0
     M10 P#<next_pocket> ; set the carousel to move to the right pocket for the selected tool
@@ -79,7 +101,7 @@ o150 if [#<selected_tool> GT 0] ; selected tool is not tool0
     M66 P1 L3 Q5 ; check carousel out position sensor
     o151 if [#5399 LT 0]
         M65 P0 ; turn off the solenoid to send atc to tool change
-        (abort, failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
+        (abort, Failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
     o151 endif
     M65 P0
 
@@ -95,7 +117,7 @@ o150 else
     M66 P0 L3 Q4 ; check carousel in position sensor
     o152 if [#5399 LT 0]
         M65 P1 ; turn off the solenoid to send atc home
-        (abort, failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
+        (abort, Failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
     o152 endif
     M65 P1
 o150 endif
@@ -112,6 +134,7 @@ o170 if [#<activate_programmable_coolant> EQ 1]
     o<program_coolant> call [#<horizontal_spindle_nozzle_dist>][#<vertical_spindle_nozzle_dist>][#<pc_angle_offset>][#<pc_tool_length>]
 o170 endif
 
+(PRINT, o<toolchange> endsub)
 o<toolchange> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_inch/unclamptool.ngc b/configs/atc_sim/macros_sim_inch/unclamptool.ngc
index 1bf17ec5..ac9c3798 100644
--- a/configs/atc_sim/macros_sim_inch/unclamptool.ngc
+++ b/configs/atc_sim/macros_sim_inch/unclamptool.ngc
@@ -4,7 +4,7 @@ M64 P2 ; unclamp the tool
 
 M66 P2 L3 Q2 ; check the unclamped tool sensor
 o100 if [#5399 LT 0]
-    (abort, failed to release tool) ; abort if the sensor does not activate in 2 seconds
+    (abort, Failed to release tool) ; abort if the sensor does not activate in 2 seconds
 o100 endif
 
 o<unclamptool> endsub [1]
diff --git a/configs/atc_sim/macros_sim_metric/clamptool.ngc b/configs/atc_sim/macros_sim_metric/clamptool.ngc
index a167d778..8b1a7b1b 100644
--- a/configs/atc_sim/macros_sim_metric/clamptool.ngc
+++ b/configs/atc_sim/macros_sim_metric/clamptool.ngc
@@ -4,7 +4,7 @@ M65 P2 ; clamp the tool
 
 M66 P5 L3 Q2 ; check the clamped tool sensor
 o100 if [#5399 LT 0]
-    (abort, failed to release tool) ; abort if the sensor does not activate in 2 seconds
+    (abort, Failed to release tool) ; abort if the sensor does not activate in 2 seconds
 o100 endif
 
 o<clamptool> endsub [1]
diff --git a/configs/atc_sim/macros_sim_metric/extendatc.ngc b/configs/atc_sim/macros_sim_metric/extendatc.ngc
index b16d6413..95828367 100644
--- a/configs/atc_sim/macros_sim_metric/extendatc.ngc
+++ b/configs/atc_sim/macros_sim_metric/extendatc.ngc
@@ -20,7 +20,7 @@ M64 P0 ; Move Carousel OUT
 M66 P1 L3 Q5 ; check for carousel out position sensor
 o100 if [#5399 LT 0]
     M65 P0 ; switch off atc out solenoid
-    (abort, atc not in position)
+    (abort, ATC not in position)
 o100 endif
 
 o<extendatc> endsub [1]
diff --git a/configs/atc_sim/macros_sim_metric/m10.ngc b/configs/atc_sim/macros_sim_metric/m10.ngc
index c02e0788..2562672e 100644
--- a/configs/atc_sim/macros_sim_metric/m10.ngc
+++ b/configs/atc_sim/macros_sim_metric/m10.ngc
@@ -4,7 +4,7 @@ o<m10> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M10> P#<p>)
+(PRINT, o<m10> P#<p>)
 
 o100 if [#3989 NE 1]
     M13
@@ -17,7 +17,7 @@ o101 if [EXISTS[#<_ini[atc]pockets>]]
 o101 endif
 
 #<steps> = [#3990 - #<p>]
-(PRINT, o<M10> P#<p>, steps=#<steps>)
+(PRINT, o<m10> P#<p>, steps=#<steps>)
 o110 if [#<steps> GT [#<number_of_pockets> / 2]]
     #<steps>=[#<steps> - #<number_of_pockets>]
 o110 endif
@@ -31,6 +31,7 @@ o130 elseif [#<steps> LT 0]
     M11 P[#<steps>]
 o130 endif
 
+(PRINT, o<m10> endsub)
 o<m10> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_metric/m11.ngc b/configs/atc_sim/macros_sim_metric/m11.ngc
index 096f578b..3db74563 100644
--- a/configs/atc_sim/macros_sim_metric/m11.ngc
+++ b/configs/atc_sim/macros_sim_metric/m11.ngc
@@ -5,7 +5,7 @@ o<m11> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M11> P#<p>)
+(PRINT, o<m11> P#<p>)
 
 o100 if [#3989 NE 1]
     (PRINT, atc not homed, homing)
@@ -34,7 +34,7 @@ o120 do
     M66 P4 L1 Q3 ; wait for rising edge on rotation index
     o130 if [#5399 LT 0]
         M65 P4 ; Stop atc motor
-        (abort, failed to get rotation index)
+        (abort, Failed to get rotation index)
     o130 endif
     #3990 = [[[#3990+2] MOD #<number_of_pockets>]-1] ; Pocket is no.1-#<number_of_pockets>
     #<steps_to_move> = [#<steps_to_move>-1]
@@ -44,6 +44,7 @@ M65 P4 ; Stop motor
 
 #<_my_current_pocket> = #3990
 
+(PRINT, o<m11> endsub)
 o<m11> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_metric/m12.ngc b/configs/atc_sim/macros_sim_metric/m12.ngc
index c1d4c3ca..34f2d6f3 100644
--- a/configs/atc_sim/macros_sim_metric/m12.ngc
+++ b/configs/atc_sim/macros_sim_metric/m12.ngc
@@ -5,7 +5,7 @@ o<m12> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M12> P#<p>)
+(PRINT, o<m12> P#<p>)
 
 o100 if [#3989 NE 1]
     (PRINT, atc not homed, homing)
@@ -34,7 +34,7 @@ o120 do
     M66 P4 L1 Q3 ; wait for rising edge on rotation index
     o130 if [#5399 LT 0]
         M65 P3 ; Stop atc motor
-        (abort, failed to get rotation index)
+        (abort, Failed to get rotation index)
     o130 endif
     #3990 = [[[#3990-2] MOD #<number_of_pockets>]+1] ; Pocket is no.1-#<number_of_pockets>
     #<steps_to_move> = [#<steps_to_move>-1]
@@ -44,6 +44,7 @@ M65 P3 ; Stop motor
 
 #<_my_current_pocket> = #3990
 
+(PRINT, o<m12> endsub)
 o<m12> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_metric/m13.ngc b/configs/atc_sim/macros_sim_metric/m13.ngc
index 06da95e4..ede03334 100644
--- a/configs/atc_sim/macros_sim_metric/m13.ngc
+++ b/configs/atc_sim/macros_sim_metric/m13.ngc
@@ -4,7 +4,7 @@ o<m13> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M13>)
+(PRINT, o<m13>)
 
 (DEBUG, EVAL[vcp.getWidget{"dynatc"}.atc_message{"REFERENCING"}])
 
@@ -12,7 +12,7 @@ M64 P3 ; Move Motor FWD
 M66 P3 L1 Q20 ; wait for rising edge on home index
 o100 if [#5399 LT 0]
     M65 P3 ; stop motor
-    (abort, failed to home carousel)
+    (abort, Failed to home carousel)
 o100 endif
 
 #3990 = 1
@@ -36,6 +36,7 @@ o120 endwhile
 
 M61 Q#3991 G43 H#3991 #5210 = 0
 
+(PRINT, o<m13> endsub)
 o<m13> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_metric/m21.ngc b/configs/atc_sim/macros_sim_metric/m21.ngc
index 9bffddd1..2ec9daea 100644
--- a/configs/atc_sim/macros_sim_metric/m21.ngc
+++ b/configs/atc_sim/macros_sim_metric/m21.ngc
@@ -5,7 +5,7 @@ o<m21> sub
 ; Parameter #3991 is used to track the current tool loaded it in the spindle (persistently)
 ; #<atc_z_tool_change_height> is the height you spindle needs to be at to clamp/unclamp a tool form the ATC (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
 ; #<atc_z_tool_clearance_height> is the clearance height you spindle needs to be at to safely clear the ATC (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)
-(PRINT, o<M21>)
+(PRINT, o<m21>)
 
 #<atc_z_tool_change_height> = -3.9000
 o101 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
@@ -29,7 +29,7 @@ M64 P0 ; Move Carousel out
 M66 P1 L3 Q5 ; check for carousel out sensor
 o100 if [#5399 LT 0]
     M65 P0 ; switch off atc out solenoid
-    (abort, atc not in position)
+    (abort, ATC not in position)
 o100 endif
 
 M24 ; activate drawbar, release the tool
@@ -39,6 +39,7 @@ G0 G53 Z#<atc_z_tool_clearance_height> ; move z to clear height
 
 #3991 = 0; save fact there is now no tool in the spindle
 
+(PRINT, o<m21> endsub)
 o<m21> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_metric/m22.ngc b/configs/atc_sim/macros_sim_metric/m22.ngc
index 825713fa..2e87c9a5 100644
--- a/configs/atc_sim/macros_sim_metric/m22.ngc
+++ b/configs/atc_sim/macros_sim_metric/m22.ngc
@@ -4,7 +4,7 @@ o<m22> sub
 ; after loading any tool in the current pocket to the spindle
 ; #<atc_z_tool_change_height> is the height you spindle needs to be at to clamp/unclamp a tool form the ATC (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
 ; #<atc_z_tool_clearance_height> is the clearance height you spindle needs to be at to safely clear the ATC (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)
-(PRINT, o<M22>)
+(PRINT, o<m22>)
 
 #<atc_z_tool_change_height> = -3.9000
 o101 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
@@ -24,18 +24,19 @@ M65 P2 ; release the drawbar to clamp the tool
 M5
 M66 P5 L3 Q1 ; check the tool clamped sensor
 o100 if [#5399 LT 0]
-    (abort, failed to reclamp tool)
+    (abort, Failed to reclamp tool)
 o100 endif
 
 M65 P0 ; Move Carousel home
 M66 P0 L3 Q4 ; check carousel in position sensor
 o110 if [#5399 LT 0]
     M65 P1 ; turn off the solenoid to send atc home
-    (abort, failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
+    (abort, Failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
 o110 endif
 
 ;M65 P1
 
+(PRINT, o<m22> endsub)
 o<m22> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_metric/m24.ngc b/configs/atc_sim/macros_sim_metric/m24.ngc
index 484fc11b..37454968 100644
--- a/configs/atc_sim/macros_sim_metric/m24.ngc
+++ b/configs/atc_sim/macros_sim_metric/m24.ngc
@@ -8,6 +8,7 @@ o100 if [#5399 LT 0]
     (abort, failed to release tool) ; abort if the sensor does not activate in 3 seconds
 o100 endif
 
+(PRINT, o<m24> endsub)
 o<m24> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_metric/m25.ngc b/configs/atc_sim/macros_sim_metric/m25.ngc
index dc55c0f5..a1a7ac98 100644
--- a/configs/atc_sim/macros_sim_metric/m25.ngc
+++ b/configs/atc_sim/macros_sim_metric/m25.ngc
@@ -6,9 +6,10 @@ M64 P0 ; Move Carousel out
 M66 P1 L3 Q5 ; check for carousel out sensor
 o100 if [#5399 LT 0]
     M65 P0 ; switch off atc out solenoid
-    (abort, atc not in position)
+    (abort, ATC not in position)
 o100 endif
 
+(PRINT, o<m25> endsub)
 o<m25> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_metric/retractatc.ngc b/configs/atc_sim/macros_sim_metric/retractatc.ngc
index 43c31da6..f9d400fe 100644
--- a/configs/atc_sim/macros_sim_metric/retractatc.ngc
+++ b/configs/atc_sim/macros_sim_metric/retractatc.ngc
@@ -6,7 +6,7 @@ M64 P1 ; Move Carousel IN
 M66 P0 L3 Q5 ; check carousel in position sensor
 o100 if [#5399 LT 0]
     M65 P1 ; turn off the solenoid to send atc home
-    (abort, failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
+    (abort, Failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
 o100 endif
 M65 P1
 
diff --git a/configs/atc_sim/macros_sim_metric/toolchange.ngc b/configs/atc_sim/macros_sim_metric/toolchange.ngc
index de63b409..21d39094 100644
--- a/configs/atc_sim/macros_sim_metric/toolchange.ngc
+++ b/configs/atc_sim/macros_sim_metric/toolchange.ngc
@@ -6,8 +6,10 @@ o<toolchange> sub
 ; Parameters #4001 to #4024 are used to track which tool is in which pocket (persistently)
 ; Parameter #4000 is not populated just used in the maths to calculate the above numbers
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
+; #<atc_z_tool_change_height> is the height you spindle needs to be at to clamp/unclamp a tool form the ATC (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
+; #<atc_z_tool_clearance_height> is the clearance height you spindle needs to be at to safely clear the ATC (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)
 
-(PRINT, o<toolchange>)
+(PRINT, o<toolchange> selected_tool: #<selected_tool>, tool_in_spindle: #<tool_in_spindle>, selected_pocket: #<selected_pocket>, current_pocket: #<current_pocket>, task: #<_task>)
 
 o100 if [#<_task> EQ 0]
     (DEBUG, Task is null)
@@ -20,28 +22,46 @@ o101 if [EXISTS[#<_ini[atc]pockets>]]
     #<number_of_pockets> = #<_ini[atc]pockets>
 o101 endif
 
+#<atc_z_tool_change_height> = -3.9000
+o102 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
+    #<atc_z_tool_change_height> = #<_ini[atc]z_tool_change_height>
+o102 endif
+#<atc_z_tool_clearance_height> = [#<_ini[AXIS_Z]MAX_LIMIT>-0.1]
+o103 if [EXISTS[#<_ini[atc]z_tool_clearance_height>]]
+    #<atc_z_tool_clearance_height> = #<_ini[atc]z_tool_clearance_height>
+o103 endif
+
 ; assign the programmable coolant parameters
 #<activate_programmable_coolant> = #1 (=0)
 #<horizontal_spindle_nozzle_dist> = #2 (=0)
 #<vertical_spindle_nozzle_dist> = #3 (=0)
 #<pc_angle_offset> = #4 (=0)
 
-; assign the variables passed by M6 to some parameters
+; assign the variables passed by M6 change_prolog to some parameters
 #100 = #<selected_tool>
 #110 = #<tool_in_spindle>
 #120 = #<selected_pocket>
+#121 = #<current_pocket>
+; NOTE:
+;      The legacy names *selected_pocket* and *current_pocket* actually reference
+;      a sequential tooldata index for tool items loaded from a tool
+;      table ([EMCIO]TOOL_TABLE) or via a tooldata database ([EMCIO]DB_PROGRAM)
 
-o110 if [#100 EQ #110] ; checks if tool in the spindle is same as requested
+o110 if [#<selected_tool> EQ #<tool_in_spindle>] ; checks if tool in the spindle is same as requested
     o<toolchange> endsub [1]
     M2
 o110 endif
 
+o111 if [#3991 NE #<tool_in_spindle>]
+    (PRINT, o<toolchange> tool_in_spindle does not match 3991)
+o111 endif
+
 #<next_pocket> = 0 ; assigns 0 to the next pocket for a later check if the tool is found in the carousel
 #<open_pocket> = 0
 #130 = #<number_of_pockets> ; assign test parameter the number of pockets in the carousel
 
 o120 do
-    o121 if [#[4000 + #130] EQ #100] ; checks all pockets to see if it contains tool number requested as the new tool
+    o121 if [#[4000 + #130] EQ #<selected_tool>] ; checks all pockets to see if it contains tool number requested as the new tool
         #<next_pocket> = #130 ; if tool is found in pocket, assigns the next pocket
     o121 endif
     o122 if [#[4000 + #130] EQ 0] ; checks if the pocket is empty, last pocket checked will be the lowest empty pocket number, for putting tool in spindle away.
@@ -50,7 +70,7 @@ o120 do
     #130 = [#130 - 1]
 o120 while [#130 GT 0]
 o130 if [#<next_pocket> EQ 0] ; if tool is not found, aborts and sends a message
-    (abort, tool not in carousel)
+    (abort, Tool T%d#<selected_tool> not found in carousel)
 o130 endif
 
 ; now we know which pocket the next tool is sitting in
@@ -59,7 +79,7 @@ o130 endif
 
 o140 if [#<tool_in_spindle> GT 0] ; checks if there is a valid tool in the spindle
     o141 if [#<open_pocket> EQ 0] ; If there is a tool in the spindle, checks if there is an open pocket
-        (abort, carousel is full, cant put away tool in into carousel)
+        (abort, Carousel is full, cant put away tool T#<tool_in_spindle> in into carousel)
     o141 endif
     M10 P[#<open_pocket>] ; move carousel to an open pocket
     M21 ; puts the tool in spindle away into the open pocket
@@ -67,10 +87,12 @@ o140 if [#<tool_in_spindle> GT 0] ; checks if there is a valid tool in the spind
     #140 = #<open_pocket>
     #[4000 + #140] = #<tool_in_spindle> ; save tool number in pocket
     #3991 = 0 ; empty tool in the spindle
+    M61 Q0
+    G49
 o140 endif
 
 G90
-G0 G53 Z0
+G0 G53 Z#<atc_z_tool_clearance_height> ; move z to clear height
 
 o150 if [#<selected_tool> GT 0] ; selected tool is not tool0
     M10 P#<next_pocket> ; set the carousel to move to the right pocket for the selected tool
@@ -79,7 +101,7 @@ o150 if [#<selected_tool> GT 0] ; selected tool is not tool0
     M66 P1 L3 Q5 ; check carousel out position sensor
     o151 if [#5399 LT 0]
         M65 P0 ; turn off the solenoid to send atc to tool change
-        (abort, failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
+        (abort, Failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
     o151 endif
     M65 P0
 
@@ -95,7 +117,7 @@ o150 else
     M66 P0 L3 Q4 ; check carousel in position sensor
     o152 if [#5399 LT 0]
         M65 P1 ; turn off the solenoid to send atc home
-        (abort, failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
+        (abort, Failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
     o152 endif
     M65 P1
 o150 endif
@@ -112,6 +134,7 @@ o170 if [#<activate_programmable_coolant> EQ 1]
     o<program_coolant> call [#<horizontal_spindle_nozzle_dist>][#<vertical_spindle_nozzle_dist>][#<pc_angle_offset>][#<pc_tool_length>]
 o170 endif
 
+(PRINT, o<toolchange> endsub)
 o<toolchange> endsub [1]
 
 M2
diff --git a/configs/atc_sim/macros_sim_metric/unclamptool.ngc b/configs/atc_sim/macros_sim_metric/unclamptool.ngc
index 1bf17ec5..ac9c3798 100644
--- a/configs/atc_sim/macros_sim_metric/unclamptool.ngc
+++ b/configs/atc_sim/macros_sim_metric/unclamptool.ngc
@@ -4,7 +4,7 @@ M64 P2 ; unclamp the tool
 
 M66 P2 L3 Q2 ; check the unclamped tool sensor
 o100 if [#5399 LT 0]
-    (abort, failed to release tool) ; abort if the sensor does not activate in 2 seconds
+    (abort, Failed to release tool) ; abort if the sensor does not activate in 2 seconds
 o100 endif
 
 o<unclamptool> endsub [1]
diff --git a/configs/probe_basic/subroutines/clamptool.ngc b/configs/probe_basic/subroutines/clamptool.ngc
index 3d1a69ab..abc300bb 100644
--- a/configs/probe_basic/subroutines/clamptool.ngc
+++ b/configs/probe_basic/subroutines/clamptool.ngc
@@ -4,7 +4,7 @@ M65 P2 ; clamp the tool
 
 M66 P5 L3 Q1 ; check the clamped tool sensor
 o100 if [#5399 LT 0]
-    (abort, failed to release tool) ; abort if the sensor does not activate in 2 seconds
+    (abort, Failed to release tool) ; abort if the sensor does not activate in 2 seconds
 o100 endif
 
 o<clamptool> endsub [1]
diff --git a/configs/probe_basic/subroutines/extendatc.ngc b/configs/probe_basic/subroutines/extendatc.ngc
index b16d6413..95828367 100644
--- a/configs/probe_basic/subroutines/extendatc.ngc
+++ b/configs/probe_basic/subroutines/extendatc.ngc
@@ -20,7 +20,7 @@ M64 P0 ; Move Carousel OUT
 M66 P1 L3 Q5 ; check for carousel out position sensor
 o100 if [#5399 LT 0]
     M65 P0 ; switch off atc out solenoid
-    (abort, atc not in position)
+    (abort, ATC not in position)
 o100 endif
 
 o<extendatc> endsub [1]
diff --git a/configs/probe_basic/subroutines/m10.ngc b/configs/probe_basic/subroutines/m10.ngc
index c02e0788..2562672e 100644
--- a/configs/probe_basic/subroutines/m10.ngc
+++ b/configs/probe_basic/subroutines/m10.ngc
@@ -4,7 +4,7 @@ o<m10> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M10> P#<p>)
+(PRINT, o<m10> P#<p>)
 
 o100 if [#3989 NE 1]
     M13
@@ -17,7 +17,7 @@ o101 if [EXISTS[#<_ini[atc]pockets>]]
 o101 endif
 
 #<steps> = [#3990 - #<p>]
-(PRINT, o<M10> P#<p>, steps=#<steps>)
+(PRINT, o<m10> P#<p>, steps=#<steps>)
 o110 if [#<steps> GT [#<number_of_pockets> / 2]]
     #<steps>=[#<steps> - #<number_of_pockets>]
 o110 endif
@@ -31,6 +31,7 @@ o130 elseif [#<steps> LT 0]
     M11 P[#<steps>]
 o130 endif
 
+(PRINT, o<m10> endsub)
 o<m10> endsub [1]
 
 M2
diff --git a/configs/probe_basic/subroutines/m11.ngc b/configs/probe_basic/subroutines/m11.ngc
index 096f578b..3db74563 100644
--- a/configs/probe_basic/subroutines/m11.ngc
+++ b/configs/probe_basic/subroutines/m11.ngc
@@ -5,7 +5,7 @@ o<m11> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M11> P#<p>)
+(PRINT, o<m11> P#<p>)
 
 o100 if [#3989 NE 1]
     (PRINT, atc not homed, homing)
@@ -34,7 +34,7 @@ o120 do
     M66 P4 L1 Q3 ; wait for rising edge on rotation index
     o130 if [#5399 LT 0]
         M65 P4 ; Stop atc motor
-        (abort, failed to get rotation index)
+        (abort, Failed to get rotation index)
     o130 endif
     #3990 = [[[#3990+2] MOD #<number_of_pockets>]-1] ; Pocket is no.1-#<number_of_pockets>
     #<steps_to_move> = [#<steps_to_move>-1]
@@ -44,6 +44,7 @@ M65 P4 ; Stop motor
 
 #<_my_current_pocket> = #3990
 
+(PRINT, o<m11> endsub)
 o<m11> endsub [1]
 
 M2
diff --git a/configs/probe_basic/subroutines/m12.ngc b/configs/probe_basic/subroutines/m12.ngc
index c1d4c3ca..34f2d6f3 100644
--- a/configs/probe_basic/subroutines/m12.ngc
+++ b/configs/probe_basic/subroutines/m12.ngc
@@ -5,7 +5,7 @@ o<m12> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M12> P#<p>)
+(PRINT, o<m12> P#<p>)
 
 o100 if [#3989 NE 1]
     (PRINT, atc not homed, homing)
@@ -34,7 +34,7 @@ o120 do
     M66 P4 L1 Q3 ; wait for rising edge on rotation index
     o130 if [#5399 LT 0]
         M65 P3 ; Stop atc motor
-        (abort, failed to get rotation index)
+        (abort, Failed to get rotation index)
     o130 endif
     #3990 = [[[#3990-2] MOD #<number_of_pockets>]+1] ; Pocket is no.1-#<number_of_pockets>
     #<steps_to_move> = [#<steps_to_move>-1]
@@ -44,6 +44,7 @@ M65 P3 ; Stop motor
 
 #<_my_current_pocket> = #3990
 
+(PRINT, o<m12> endsub)
 o<m12> endsub [1]
 
 M2
diff --git a/configs/probe_basic/subroutines/m13.ngc b/configs/probe_basic/subroutines/m13.ngc
index 06da95e4..ede03334 100644
--- a/configs/probe_basic/subroutines/m13.ngc
+++ b/configs/probe_basic/subroutines/m13.ngc
@@ -4,7 +4,7 @@ o<m13> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M13>)
+(PRINT, o<m13>)
 
 (DEBUG, EVAL[vcp.getWidget{"dynatc"}.atc_message{"REFERENCING"}])
 
@@ -12,7 +12,7 @@ M64 P3 ; Move Motor FWD
 M66 P3 L1 Q20 ; wait for rising edge on home index
 o100 if [#5399 LT 0]
     M65 P3 ; stop motor
-    (abort, failed to home carousel)
+    (abort, Failed to home carousel)
 o100 endif
 
 #3990 = 1
@@ -36,6 +36,7 @@ o120 endwhile
 
 M61 Q#3991 G43 H#3991 #5210 = 0
 
+(PRINT, o<m13> endsub)
 o<m13> endsub [1]
 
 M2
diff --git a/configs/probe_basic/subroutines/m21.ngc b/configs/probe_basic/subroutines/m21.ngc
index 17152078..72481562 100644
--- a/configs/probe_basic/subroutines/m21.ngc
+++ b/configs/probe_basic/subroutines/m21.ngc
@@ -5,7 +5,7 @@ o<m21> sub
 ; Parameter #3991 is used to track the current tool loaded it in the spindle (persistently)
 ; #<atc_z_tool_change_height> is the height you spindle needs to be at to clamp/unclamp a tool form the ATC (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
 ; #<atc_z_tool_clearance_height> is the clearance height you spindle needs to be at to safely clear the ATC (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)
-(PRINT, o<M21>)
+(PRINT, o<m21>)
 
 #<atc_z_tool_change_height> = -3.1900
 o101 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
@@ -29,7 +29,7 @@ M64 P0 ; Move Carousel out
 M66 P1 L3 Q5 ; check for carousel out sensor
 o100 if [#5399 LT 0]
     M65 P0 ; switch off atc out solenoid
-    (abort, atc not in position)
+    (abort, ATC not in position)
 o100 endif
 
 M24 ; activate drawbar, release the tool
@@ -39,6 +39,7 @@ G0 G53 Z#<atc_z_tool_clearance_height> ; move z to clear height
 
 #3991 = 0; save fact there is now no tool in the spindle
 
+(PRINT, o<m21> endsub)
 o<m21> endsub [1]
 
 M2
diff --git a/configs/probe_basic/subroutines/m22.ngc b/configs/probe_basic/subroutines/m22.ngc
index 3150f732..2de79912 100644
--- a/configs/probe_basic/subroutines/m22.ngc
+++ b/configs/probe_basic/subroutines/m22.ngc
@@ -4,7 +4,7 @@ o<m22> sub
 ; after loading any tool in the current pocket to the spindle
 ; #<atc_z_tool_change_height> is the height you spindle needs to be at to clamp/unclamp a tool form the ATC (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
 ; #<atc_z_tool_clearance_height> is the clearance height you spindle needs to be at to safely clear the ATC (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)
-(PRINT, o<M22>)
+(PRINT, o<m22>)
 
 #<atc_z_tool_change_height> = -3.1900
 o101 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
@@ -24,18 +24,19 @@ M65 P2 ; release the drawbar to clamp the tool
 M5
 M66 P5 L3 Q1 ; check the tool clamped sensor
 o100 if [#5399 LT 0]
-    (abort, failed to reclamp tool)
+    (abort, Failed to reclamp tool)
 o100 endif
 
 M65 P0 ; Move Carousel home
 M66 P0 L3 Q4 ; check carousel in position sensor
 o110 if [#5399 LT 0]
     M65 P1 ; turn off the solenoid to send atc home
-    (abort, failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
+    (abort, Failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
 o110 endif
 
 ;M65 P1
 
+(PRINT, o<m22> endsub)
 o<m22> endsub [1]
 
 M2
diff --git a/configs/probe_basic/subroutines/m24.ngc b/configs/probe_basic/subroutines/m24.ngc
index 263b5278..37454968 100644
--- a/configs/probe_basic/subroutines/m24.ngc
+++ b/configs/probe_basic/subroutines/m24.ngc
@@ -2,12 +2,13 @@ o<m24> sub
 (PRINT, o<m24>)
 
 M64 P2 ; unclamp the tool
-M66 P2 L3 Q3 ; check the unclamped tool sensor
+M66 P2 L3 Q2 ; check the unclamped tool sensor
 o100 if [#5399 LT 0]
     M65 P2 ; turn off the drawbar
     (abort, failed to release tool) ; abort if the sensor does not activate in 3 seconds
 o100 endif
 
+(PRINT, o<m24> endsub)
 o<m24> endsub [1]
 
 M2
diff --git a/configs/probe_basic/subroutines/m25.ngc b/configs/probe_basic/subroutines/m25.ngc
index dc55c0f5..a1a7ac98 100644
--- a/configs/probe_basic/subroutines/m25.ngc
+++ b/configs/probe_basic/subroutines/m25.ngc
@@ -6,9 +6,10 @@ M64 P0 ; Move Carousel out
 M66 P1 L3 Q5 ; check for carousel out sensor
 o100 if [#5399 LT 0]
     M65 P0 ; switch off atc out solenoid
-    (abort, atc not in position)
+    (abort, ATC not in position)
 o100 endif
 
+(PRINT, o<m25> endsub)
 o<m25> endsub [1]
 
 M2
diff --git a/configs/probe_basic/subroutines/retractatc.ngc b/configs/probe_basic/subroutines/retractatc.ngc
index 717324b9..4219ca38 100644
--- a/configs/probe_basic/subroutines/retractatc.ngc
+++ b/configs/probe_basic/subroutines/retractatc.ngc
@@ -6,7 +6,7 @@ M64 P1 ; Move Carousel IN
 M66 P0 L3 Q4 ; check carousel in position sensor
 o100 if [#5399 LT 0]
     M65 P1 ; turn off the solenoid to send atc home
-    (abort, failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
+    (abort, Failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
 o100 endif
 M65 P1
 
diff --git a/configs/probe_basic/subroutines/toolchange.ngc b/configs/probe_basic/subroutines/toolchange.ngc
index de63b409..b8cfb5ef 100644
--- a/configs/probe_basic/subroutines/toolchange.ngc
+++ b/configs/probe_basic/subroutines/toolchange.ngc
@@ -6,8 +6,10 @@ o<toolchange> sub
 ; Parameters #4001 to #4024 are used to track which tool is in which pocket (persistently)
 ; Parameter #4000 is not populated just used in the maths to calculate the above numbers
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
+; #<atc_z_tool_change_height> is the height you spindle needs to be at to clamp/unclamp a tool form the ATC (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
+; #<atc_z_tool_clearance_height> is the clearance height you spindle needs to be at to safely clear the ATC (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)
 
-(PRINT, o<toolchange>)
+(PRINT, o<toolchange> selected_tool: #<selected_tool>, tool_in_spindle: #<tool_in_spindle>, selected_pocket: #<selected_pocket>, current_pocket: #<current_pocket>, task: #<_task>)
 
 o100 if [#<_task> EQ 0]
     (DEBUG, Task is null)
@@ -20,28 +22,46 @@ o101 if [EXISTS[#<_ini[atc]pockets>]]
     #<number_of_pockets> = #<_ini[atc]pockets>
 o101 endif
 
+#<atc_z_tool_change_height> = -3.1900
+o102 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
+    #<atc_z_tool_change_height> = #<_ini[atc]z_tool_change_height>
+o102 endif
+#<atc_z_tool_clearance_height> = [#<_ini[AXIS_Z]MAX_LIMIT>-0.1]
+o103 if [EXISTS[#<_ini[atc]z_tool_clearance_height>]]
+    #<atc_z_tool_clearance_height> = #<_ini[atc]z_tool_clearance_height>
+o103 endif
+
 ; assign the programmable coolant parameters
 #<activate_programmable_coolant> = #1 (=0)
 #<horizontal_spindle_nozzle_dist> = #2 (=0)
 #<vertical_spindle_nozzle_dist> = #3 (=0)
 #<pc_angle_offset> = #4 (=0)
 
-; assign the variables passed by M6 to some parameters
+; assign the variables passed by M6 change_prolog to some parameters
 #100 = #<selected_tool>
 #110 = #<tool_in_spindle>
 #120 = #<selected_pocket>
+#121 = #<current_pocket>
+; NOTE:
+;      The legacy names *selected_pocket* and *current_pocket* actually reference
+;      a sequential tooldata index for tool items loaded from a tool
+;      table ([EMCIO]TOOL_TABLE) or via a tooldata database ([EMCIO]DB_PROGRAM)
 
-o110 if [#100 EQ #110] ; checks if tool in the spindle is same as requested
+o110 if [#<selected_tool> EQ #<tool_in_spindle>] ; checks if tool in the spindle is same as requested
     o<toolchange> endsub [1]
     M2
 o110 endif
 
+o111 if [#3991 NE #<tool_in_spindle>]
+    (PRINT, o<toolchange> tool_in_spindle does not match 3991)
+o111 endif
+
 #<next_pocket> = 0 ; assigns 0 to the next pocket for a later check if the tool is found in the carousel
 #<open_pocket> = 0
 #130 = #<number_of_pockets> ; assign test parameter the number of pockets in the carousel
 
 o120 do
-    o121 if [#[4000 + #130] EQ #100] ; checks all pockets to see if it contains tool number requested as the new tool
+    o121 if [#[4000 + #130] EQ #<selected_tool>] ; checks all pockets to see if it contains tool number requested as the new tool
         #<next_pocket> = #130 ; if tool is found in pocket, assigns the next pocket
     o121 endif
     o122 if [#[4000 + #130] EQ 0] ; checks if the pocket is empty, last pocket checked will be the lowest empty pocket number, for putting tool in spindle away.
@@ -50,7 +70,7 @@ o120 do
     #130 = [#130 - 1]
 o120 while [#130 GT 0]
 o130 if [#<next_pocket> EQ 0] ; if tool is not found, aborts and sends a message
-    (abort, tool not in carousel)
+    (abort, Tool T%d#<selected_tool> not found in carousel)
 o130 endif
 
 ; now we know which pocket the next tool is sitting in
@@ -59,7 +79,7 @@ o130 endif
 
 o140 if [#<tool_in_spindle> GT 0] ; checks if there is a valid tool in the spindle
     o141 if [#<open_pocket> EQ 0] ; If there is a tool in the spindle, checks if there is an open pocket
-        (abort, carousel is full, cant put away tool in into carousel)
+        (abort, Carousel is full, cant put away tool T#<tool_in_spindle> in into carousel)
     o141 endif
     M10 P[#<open_pocket>] ; move carousel to an open pocket
     M21 ; puts the tool in spindle away into the open pocket
@@ -67,10 +87,12 @@ o140 if [#<tool_in_spindle> GT 0] ; checks if there is a valid tool in the spind
     #140 = #<open_pocket>
     #[4000 + #140] = #<tool_in_spindle> ; save tool number in pocket
     #3991 = 0 ; empty tool in the spindle
+    M61 Q0
+    G49
 o140 endif
 
 G90
-G0 G53 Z0
+G0 G53 Z#<atc_z_tool_clearance_height> ; move z to clear height
 
 o150 if [#<selected_tool> GT 0] ; selected tool is not tool0
     M10 P#<next_pocket> ; set the carousel to move to the right pocket for the selected tool
@@ -79,7 +101,7 @@ o150 if [#<selected_tool> GT 0] ; selected tool is not tool0
     M66 P1 L3 Q5 ; check carousel out position sensor
     o151 if [#5399 LT 0]
         M65 P0 ; turn off the solenoid to send atc to tool change
-        (abort, failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
+        (abort, Failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
     o151 endif
     M65 P0
 
@@ -95,7 +117,7 @@ o150 else
     M66 P0 L3 Q4 ; check carousel in position sensor
     o152 if [#5399 LT 0]
         M65 P1 ; turn off the solenoid to send atc home
-        (abort, failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
+        (abort, Failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
     o152 endif
     M65 P1
 o150 endif
@@ -112,6 +134,7 @@ o170 if [#<activate_programmable_coolant> EQ 1]
     o<program_coolant> call [#<horizontal_spindle_nozzle_dist>][#<vertical_spindle_nozzle_dist>][#<pc_angle_offset>][#<pc_tool_length>]
 o170 endif
 
+(PRINT, o<toolchange> endsub)
 o<toolchange> endsub [1]
 
 M2
diff --git a/configs/probe_basic/subroutines/unclamptool.ngc b/configs/probe_basic/subroutines/unclamptool.ngc
index 1bf17ec5..ac9c3798 100644
--- a/configs/probe_basic/subroutines/unclamptool.ngc
+++ b/configs/probe_basic/subroutines/unclamptool.ngc
@@ -4,7 +4,7 @@ M64 P2 ; unclamp the tool
 
 M66 P2 L3 Q2 ; check the unclamped tool sensor
 o100 if [#5399 LT 0]
-    (abort, failed to release tool) ; abort if the sensor does not activate in 2 seconds
+    (abort, Failed to release tool) ; abort if the sensor does not activate in 2 seconds
 o100 endif
 
 o<unclamptool> endsub [1]
diff --git a/configs/probe_basic_lathe/subroutines/clamptool.ngc b/configs/probe_basic_lathe/subroutines/clamptool.ngc
index a167d778..8b1a7b1b 100644
--- a/configs/probe_basic_lathe/subroutines/clamptool.ngc
+++ b/configs/probe_basic_lathe/subroutines/clamptool.ngc
@@ -4,7 +4,7 @@ M65 P2 ; clamp the tool
 
 M66 P5 L3 Q2 ; check the clamped tool sensor
 o100 if [#5399 LT 0]
-    (abort, failed to release tool) ; abort if the sensor does not activate in 2 seconds
+    (abort, Failed to release tool) ; abort if the sensor does not activate in 2 seconds
 o100 endif
 
 o<clamptool> endsub [1]
diff --git a/configs/probe_basic_lathe/subroutines/extendatc.ngc b/configs/probe_basic_lathe/subroutines/extendatc.ngc
index b16d6413..95828367 100644
--- a/configs/probe_basic_lathe/subroutines/extendatc.ngc
+++ b/configs/probe_basic_lathe/subroutines/extendatc.ngc
@@ -20,7 +20,7 @@ M64 P0 ; Move Carousel OUT
 M66 P1 L3 Q5 ; check for carousel out position sensor
 o100 if [#5399 LT 0]
     M65 P0 ; switch off atc out solenoid
-    (abort, atc not in position)
+    (abort, ATC not in position)
 o100 endif
 
 o<extendatc> endsub [1]
diff --git a/configs/probe_basic_lathe/subroutines/m10.ngc b/configs/probe_basic_lathe/subroutines/m10.ngc
index c02e0788..2562672e 100644
--- a/configs/probe_basic_lathe/subroutines/m10.ngc
+++ b/configs/probe_basic_lathe/subroutines/m10.ngc
@@ -4,7 +4,7 @@ o<m10> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M10> P#<p>)
+(PRINT, o<m10> P#<p>)
 
 o100 if [#3989 NE 1]
     M13
@@ -17,7 +17,7 @@ o101 if [EXISTS[#<_ini[atc]pockets>]]
 o101 endif
 
 #<steps> = [#3990 - #<p>]
-(PRINT, o<M10> P#<p>, steps=#<steps>)
+(PRINT, o<m10> P#<p>, steps=#<steps>)
 o110 if [#<steps> GT [#<number_of_pockets> / 2]]
     #<steps>=[#<steps> - #<number_of_pockets>]
 o110 endif
@@ -31,6 +31,7 @@ o130 elseif [#<steps> LT 0]
     M11 P[#<steps>]
 o130 endif
 
+(PRINT, o<m10> endsub)
 o<m10> endsub [1]
 
 M2
diff --git a/configs/probe_basic_lathe/subroutines/m11.ngc b/configs/probe_basic_lathe/subroutines/m11.ngc
index 096f578b..3db74563 100644
--- a/configs/probe_basic_lathe/subroutines/m11.ngc
+++ b/configs/probe_basic_lathe/subroutines/m11.ngc
@@ -5,7 +5,7 @@ o<m11> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M11> P#<p>)
+(PRINT, o<m11> P#<p>)
 
 o100 if [#3989 NE 1]
     (PRINT, atc not homed, homing)
@@ -34,7 +34,7 @@ o120 do
     M66 P4 L1 Q3 ; wait for rising edge on rotation index
     o130 if [#5399 LT 0]
         M65 P4 ; Stop atc motor
-        (abort, failed to get rotation index)
+        (abort, Failed to get rotation index)
     o130 endif
     #3990 = [[[#3990+2] MOD #<number_of_pockets>]-1] ; Pocket is no.1-#<number_of_pockets>
     #<steps_to_move> = [#<steps_to_move>-1]
@@ -44,6 +44,7 @@ M65 P4 ; Stop motor
 
 #<_my_current_pocket> = #3990
 
+(PRINT, o<m11> endsub)
 o<m11> endsub [1]
 
 M2
diff --git a/configs/probe_basic_lathe/subroutines/m12.ngc b/configs/probe_basic_lathe/subroutines/m12.ngc
index c1d4c3ca..34f2d6f3 100644
--- a/configs/probe_basic_lathe/subroutines/m12.ngc
+++ b/configs/probe_basic_lathe/subroutines/m12.ngc
@@ -5,7 +5,7 @@ o<m12> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M12> P#<p>)
+(PRINT, o<m12> P#<p>)
 
 o100 if [#3989 NE 1]
     (PRINT, atc not homed, homing)
@@ -34,7 +34,7 @@ o120 do
     M66 P4 L1 Q3 ; wait for rising edge on rotation index
     o130 if [#5399 LT 0]
         M65 P3 ; Stop atc motor
-        (abort, failed to get rotation index)
+        (abort, Failed to get rotation index)
     o130 endif
     #3990 = [[[#3990-2] MOD #<number_of_pockets>]+1] ; Pocket is no.1-#<number_of_pockets>
     #<steps_to_move> = [#<steps_to_move>-1]
@@ -44,6 +44,7 @@ M65 P3 ; Stop motor
 
 #<_my_current_pocket> = #3990
 
+(PRINT, o<m12> endsub)
 o<m12> endsub [1]
 
 M2
diff --git a/configs/probe_basic_lathe/subroutines/m13.ngc b/configs/probe_basic_lathe/subroutines/m13.ngc
index 06da95e4..ede03334 100644
--- a/configs/probe_basic_lathe/subroutines/m13.ngc
+++ b/configs/probe_basic_lathe/subroutines/m13.ngc
@@ -4,7 +4,7 @@ o<m13> sub
 ; Parameter #3989 is used to track if the carousel is homed (M13) (volatile)
 ; Parameter #3990 is used to track the current tool pocket (persistently)
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
-(PRINT, o<M13>)
+(PRINT, o<m13>)
 
 (DEBUG, EVAL[vcp.getWidget{"dynatc"}.atc_message{"REFERENCING"}])
 
@@ -12,7 +12,7 @@ M64 P3 ; Move Motor FWD
 M66 P3 L1 Q20 ; wait for rising edge on home index
 o100 if [#5399 LT 0]
     M65 P3 ; stop motor
-    (abort, failed to home carousel)
+    (abort, Failed to home carousel)
 o100 endif
 
 #3990 = 1
@@ -36,6 +36,7 @@ o120 endwhile
 
 M61 Q#3991 G43 H#3991 #5210 = 0
 
+(PRINT, o<m13> endsub)
 o<m13> endsub [1]
 
 M2
diff --git a/configs/probe_basic_lathe/subroutines/m21.ngc b/configs/probe_basic_lathe/subroutines/m21.ngc
index 9bffddd1..2ec9daea 100644
--- a/configs/probe_basic_lathe/subroutines/m21.ngc
+++ b/configs/probe_basic_lathe/subroutines/m21.ngc
@@ -5,7 +5,7 @@ o<m21> sub
 ; Parameter #3991 is used to track the current tool loaded it in the spindle (persistently)
 ; #<atc_z_tool_change_height> is the height you spindle needs to be at to clamp/unclamp a tool form the ATC (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
 ; #<atc_z_tool_clearance_height> is the clearance height you spindle needs to be at to safely clear the ATC (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)
-(PRINT, o<M21>)
+(PRINT, o<m21>)
 
 #<atc_z_tool_change_height> = -3.9000
 o101 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
@@ -29,7 +29,7 @@ M64 P0 ; Move Carousel out
 M66 P1 L3 Q5 ; check for carousel out sensor
 o100 if [#5399 LT 0]
     M65 P0 ; switch off atc out solenoid
-    (abort, atc not in position)
+    (abort, ATC not in position)
 o100 endif
 
 M24 ; activate drawbar, release the tool
@@ -39,6 +39,7 @@ G0 G53 Z#<atc_z_tool_clearance_height> ; move z to clear height
 
 #3991 = 0; save fact there is now no tool in the spindle
 
+(PRINT, o<m21> endsub)
 o<m21> endsub [1]
 
 M2
diff --git a/configs/probe_basic_lathe/subroutines/m22.ngc b/configs/probe_basic_lathe/subroutines/m22.ngc
index 825713fa..2e87c9a5 100644
--- a/configs/probe_basic_lathe/subroutines/m22.ngc
+++ b/configs/probe_basic_lathe/subroutines/m22.ngc
@@ -4,7 +4,7 @@ o<m22> sub
 ; after loading any tool in the current pocket to the spindle
 ; #<atc_z_tool_change_height> is the height you spindle needs to be at to clamp/unclamp a tool form the ATC (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
 ; #<atc_z_tool_clearance_height> is the clearance height you spindle needs to be at to safely clear the ATC (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)
-(PRINT, o<M22>)
+(PRINT, o<m22>)
 
 #<atc_z_tool_change_height> = -3.9000
 o101 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
@@ -24,18 +24,19 @@ M65 P2 ; release the drawbar to clamp the tool
 M5
 M66 P5 L3 Q1 ; check the tool clamped sensor
 o100 if [#5399 LT 0]
-    (abort, failed to reclamp tool)
+    (abort, Failed to reclamp tool)
 o100 endif
 
 M65 P0 ; Move Carousel home
 M66 P0 L3 Q4 ; check carousel in position sensor
 o110 if [#5399 LT 0]
     M65 P1 ; turn off the solenoid to send atc home
-    (abort, failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
+    (abort, Failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
 o110 endif
 
 ;M65 P1
 
+(PRINT, o<m22> endsub)
 o<m22> endsub [1]
 
 M2
diff --git a/configs/probe_basic_lathe/subroutines/m24.ngc b/configs/probe_basic_lathe/subroutines/m24.ngc
index 263b5278..37454968 100644
--- a/configs/probe_basic_lathe/subroutines/m24.ngc
+++ b/configs/probe_basic_lathe/subroutines/m24.ngc
@@ -2,12 +2,13 @@ o<m24> sub
 (PRINT, o<m24>)
 
 M64 P2 ; unclamp the tool
-M66 P2 L3 Q3 ; check the unclamped tool sensor
+M66 P2 L3 Q2 ; check the unclamped tool sensor
 o100 if [#5399 LT 0]
     M65 P2 ; turn off the drawbar
     (abort, failed to release tool) ; abort if the sensor does not activate in 3 seconds
 o100 endif
 
+(PRINT, o<m24> endsub)
 o<m24> endsub [1]
 
 M2
diff --git a/configs/probe_basic_lathe/subroutines/m25.ngc b/configs/probe_basic_lathe/subroutines/m25.ngc
index dc55c0f5..a1a7ac98 100644
--- a/configs/probe_basic_lathe/subroutines/m25.ngc
+++ b/configs/probe_basic_lathe/subroutines/m25.ngc
@@ -6,9 +6,10 @@ M64 P0 ; Move Carousel out
 M66 P1 L3 Q5 ; check for carousel out sensor
 o100 if [#5399 LT 0]
     M65 P0 ; switch off atc out solenoid
-    (abort, atc not in position)
+    (abort, ATC not in position)
 o100 endif
 
+(PRINT, o<m25> endsub)
 o<m25> endsub [1]
 
 M2
diff --git a/configs/probe_basic_lathe/subroutines/retractatc.ngc b/configs/probe_basic_lathe/subroutines/retractatc.ngc
index 43c31da6..f9d400fe 100644
--- a/configs/probe_basic_lathe/subroutines/retractatc.ngc
+++ b/configs/probe_basic_lathe/subroutines/retractatc.ngc
@@ -6,7 +6,7 @@ M64 P1 ; Move Carousel IN
 M66 P0 L3 Q5 ; check carousel in position sensor
 o100 if [#5399 LT 0]
     M65 P1 ; turn off the solenoid to send atc home
-    (abort, failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
+    (abort, Failed to send carousel home) ; abort if the sensor does not activate in 5 seconds
 o100 endif
 M65 P1
 
diff --git a/configs/probe_basic_lathe/subroutines/toolchange.ngc b/configs/probe_basic_lathe/subroutines/toolchange.ngc
index de63b409..21d39094 100644
--- a/configs/probe_basic_lathe/subroutines/toolchange.ngc
+++ b/configs/probe_basic_lathe/subroutines/toolchange.ngc
@@ -6,8 +6,10 @@ o<toolchange> sub
 ; Parameters #4001 to #4024 are used to track which tool is in which pocket (persistently)
 ; Parameter #4000 is not populated just used in the maths to calculate the above numbers
 ; #<number_of_pockets>: The number of pockets in the ATC is automaticity pulled from the INI via #<_ini[atc]pockets>
+; #<atc_z_tool_change_height> is the height you spindle needs to be at to clamp/unclamp a tool form the ATC (Set via INI [ATC]Z_TOOL_CHANGE_HEIGHT)
+; #<atc_z_tool_clearance_height> is the clearance height you spindle needs to be at to safely clear the ATC (Set via INI [ATC]Z_TOOL_CLEARANCE_HEIGHT)
 
-(PRINT, o<toolchange>)
+(PRINT, o<toolchange> selected_tool: #<selected_tool>, tool_in_spindle: #<tool_in_spindle>, selected_pocket: #<selected_pocket>, current_pocket: #<current_pocket>, task: #<_task>)
 
 o100 if [#<_task> EQ 0]
     (DEBUG, Task is null)
@@ -20,28 +22,46 @@ o101 if [EXISTS[#<_ini[atc]pockets>]]
     #<number_of_pockets> = #<_ini[atc]pockets>
 o101 endif
 
+#<atc_z_tool_change_height> = -3.9000
+o102 if [EXISTS[#<_ini[atc]z_tool_change_height>]]
+    #<atc_z_tool_change_height> = #<_ini[atc]z_tool_change_height>
+o102 endif
+#<atc_z_tool_clearance_height> = [#<_ini[AXIS_Z]MAX_LIMIT>-0.1]
+o103 if [EXISTS[#<_ini[atc]z_tool_clearance_height>]]
+    #<atc_z_tool_clearance_height> = #<_ini[atc]z_tool_clearance_height>
+o103 endif
+
 ; assign the programmable coolant parameters
 #<activate_programmable_coolant> = #1 (=0)
 #<horizontal_spindle_nozzle_dist> = #2 (=0)
 #<vertical_spindle_nozzle_dist> = #3 (=0)
 #<pc_angle_offset> = #4 (=0)
 
-; assign the variables passed by M6 to some parameters
+; assign the variables passed by M6 change_prolog to some parameters
 #100 = #<selected_tool>
 #110 = #<tool_in_spindle>
 #120 = #<selected_pocket>
+#121 = #<current_pocket>
+; NOTE:
+;      The legacy names *selected_pocket* and *current_pocket* actually reference
+;      a sequential tooldata index for tool items loaded from a tool
+;      table ([EMCIO]TOOL_TABLE) or via a tooldata database ([EMCIO]DB_PROGRAM)
 
-o110 if [#100 EQ #110] ; checks if tool in the spindle is same as requested
+o110 if [#<selected_tool> EQ #<tool_in_spindle>] ; checks if tool in the spindle is same as requested
     o<toolchange> endsub [1]
     M2
 o110 endif
 
+o111 if [#3991 NE #<tool_in_spindle>]
+    (PRINT, o<toolchange> tool_in_spindle does not match 3991)
+o111 endif
+
 #<next_pocket> = 0 ; assigns 0 to the next pocket for a later check if the tool is found in the carousel
 #<open_pocket> = 0
 #130 = #<number_of_pockets> ; assign test parameter the number of pockets in the carousel
 
 o120 do
-    o121 if [#[4000 + #130] EQ #100] ; checks all pockets to see if it contains tool number requested as the new tool
+    o121 if [#[4000 + #130] EQ #<selected_tool>] ; checks all pockets to see if it contains tool number requested as the new tool
         #<next_pocket> = #130 ; if tool is found in pocket, assigns the next pocket
     o121 endif
     o122 if [#[4000 + #130] EQ 0] ; checks if the pocket is empty, last pocket checked will be the lowest empty pocket number, for putting tool in spindle away.
@@ -50,7 +70,7 @@ o120 do
     #130 = [#130 - 1]
 o120 while [#130 GT 0]
 o130 if [#<next_pocket> EQ 0] ; if tool is not found, aborts and sends a message
-    (abort, tool not in carousel)
+    (abort, Tool T%d#<selected_tool> not found in carousel)
 o130 endif
 
 ; now we know which pocket the next tool is sitting in
@@ -59,7 +79,7 @@ o130 endif
 
 o140 if [#<tool_in_spindle> GT 0] ; checks if there is a valid tool in the spindle
     o141 if [#<open_pocket> EQ 0] ; If there is a tool in the spindle, checks if there is an open pocket
-        (abort, carousel is full, cant put away tool in into carousel)
+        (abort, Carousel is full, cant put away tool T#<tool_in_spindle> in into carousel)
     o141 endif
     M10 P[#<open_pocket>] ; move carousel to an open pocket
     M21 ; puts the tool in spindle away into the open pocket
@@ -67,10 +87,12 @@ o140 if [#<tool_in_spindle> GT 0] ; checks if there is a valid tool in the spind
     #140 = #<open_pocket>
     #[4000 + #140] = #<tool_in_spindle> ; save tool number in pocket
     #3991 = 0 ; empty tool in the spindle
+    M61 Q0
+    G49
 o140 endif
 
 G90
-G0 G53 Z0
+G0 G53 Z#<atc_z_tool_clearance_height> ; move z to clear height
 
 o150 if [#<selected_tool> GT 0] ; selected tool is not tool0
     M10 P#<next_pocket> ; set the carousel to move to the right pocket for the selected tool
@@ -79,7 +101,7 @@ o150 if [#<selected_tool> GT 0] ; selected tool is not tool0
     M66 P1 L3 Q5 ; check carousel out position sensor
     o151 if [#5399 LT 0]
         M65 P0 ; turn off the solenoid to send atc to tool change
-        (abort, failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
+        (abort, Failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
     o151 endif
     M65 P0
 
@@ -95,7 +117,7 @@ o150 else
     M66 P0 L3 Q4 ; check carousel in position sensor
     o152 if [#5399 LT 0]
         M65 P1 ; turn off the solenoid to send atc home
-        (abort, failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
+        (abort, Failed to send carousel home) ; abort if the    sensor does not activate in 5 seconds
     o152 endif
     M65 P1
 o150 endif
@@ -112,6 +134,7 @@ o170 if [#<activate_programmable_coolant> EQ 1]
     o<program_coolant> call [#<horizontal_spindle_nozzle_dist>][#<vertical_spindle_nozzle_dist>][#<pc_angle_offset>][#<pc_tool_length>]
 o170 endif
 
+(PRINT, o<toolchange> endsub)
 o<toolchange> endsub [1]
 
 M2
diff --git a/configs/probe_basic_lathe/subroutines/unclamptool.ngc b/configs/probe_basic_lathe/subroutines/unclamptool.ngc
index 1bf17ec5..ac9c3798 100644
--- a/configs/probe_basic_lathe/subroutines/unclamptool.ngc
+++ b/configs/probe_basic_lathe/subroutines/unclamptool.ngc
@@ -4,7 +4,7 @@ M64 P2 ; unclamp the tool
 
 M66 P2 L3 Q2 ; check the unclamped tool sensor
 o100 if [#5399 LT 0]
-    (abort, failed to release tool) ; abort if the sensor does not activate in 2 seconds
+    (abort, Failed to release tool) ; abort if the sensor does not activate in 2 seconds
 o100 endif
 
 o<unclamptool> endsub [1]