Manual tool change + tool lengh touch off

( see: http://www.linuxcnc.org/index.php/english/forum/10-advanced-configuration/5596-manual-tool-change--tool-lengh-touch-off?start=120#227008 )

( Filename: tool-change.ngc )
( LinuxCNC Manual Tool-Change Subroutines for Milling Machines version 1.1: subroutine 1/2 )
( BEFORE USING CHANGE "CONFIGURATION PARAMETERS" BELOW FOR YOUR MACHINE! )
(  )

( In the LinuxCNC .ini config file, under the [RS274NGC] section add: )
(    # change/add/use SUBROUTINE_PATH to point to the location where these tool-change subroutines are located: )
(    SUBROUTINE_PATH = macros )
(    REMAP=M6    modalgroup=6 ngc=tool-change )

( and under the [EMCIO] section add: )
(    TOOL_CHANGE_AT_G30 = 0 )

( and ensure neither TOOL_CHANGE_POSITION nor TOOL_CHANGE_QUILL_UP is set. )

( In the LinuxCNC .hal config file, map some input pin to be the probe input, e.g.: )
(    net probe-z parport.0.pin-12-in => motion.probe-input )
( For multiple probe inputs, the following can be used, in this case a MESA 7i96 is controlling a NO probe, and a NC Toolsetter )
( net probe-in <= hm2_7i96.0.gpio.005.in )
( net length-in <= hm2_7i96.0.gpio.009.in_not )
( net probe-in => or2.0.in0 )
( net length-in => or2.0.in1 )
( net probe-or-length <= or2.0.out => motion.probe-input )
( The following tool change pins are required as well; )
( loadusr -W hal_manualtoolchange )
( net tool-change iocontrol.0.tool-change => hal_manualtoolchange.change )
( net tool-changed iocontrol.0.tool-changed <= hal_manualtoolchange.changed )
( net tool-number iocontrol.0.tool-prep-number => hal_manualtoolchange.number )
( net tool-prepare-loopback iocontrol.0.tool-prepare => iocontrol.0.tool-prepared )

( In addition I have had to modify the post-processor for the following; )
( Eliminate G43 call outs after tool changes )
( Use G53 instead of G28, typically just a check box when post processing )
( Set machine home for Z in F360 machine setup to be -.250, my machine Zero is at top of travel )

( Usage: M6 in the g-code will invoke a manual tool change with automatic tool height adjustment. )

( My sequence of Running; )
( 1. Use a T99, a .250" Rod, to touch off X, Y, and Z of WCS as modeled in CAM )
( 2. Manually call an M6 T99, tool will touch off on toolsetter and store tool height )
( 3. Load program and run, M6 in program will prompt tool change, after changing, tool will touch off and Z will adjust )
( 4. Run any and all code using the same desired Z height for the WCS as designed )

( General theory of operation: touches each tool off to the tool height sensor. The first tool is used as the reference, all   )
(     subsequent tools adjust the tool offset.                                                                                 )
(     It is moved away by raising Z to _TravelZ before moving towards the switch, and when                                     )
(     moving back from the switch again moves at height _TravelZ before going straight back down to the original position. Set )
(     all necessary modes to ensure correct operation no matter what state the program is in when this is called. We eliminate )
(     almost all side effects by saving and restoring the modal state. )
(  )

O<tool_change> SUB
                                                       (PRINT,tool change routine BEGIN)
                                                       (PRINT,current tool is #<_current_tool>)
                                                       (PRINT,selected tool is #<_selected_tool>)
O100 IF [ #<_selected_tool> EQ #<_current_tool> ]
#4921 = 0                                              (PRINT,LinuxCNC starting, initialize value)
O100 ELSE
                                                       (PRINT,save current position in WCS )
#<xpos> = #<_x>                                        (PRINT,X=#<_x>)
#<ypos> = #<_y>                                        (PRINT,X=#<_y>)
#<prevtool> = #<_current_tool>

#<tsx> = #<_ini[TOOLSENSOR]X>
#<tsy> = #<_ini[TOOLSENSOR]Y>
#<tsz> = #<_ini[TOOLSENSOR]Z>
#<trv> = #<_ini[TOOLSENSOR]TRAVELZ>
#<chx> = #<_ini[CHANGE_POSITION]X>
#<chy> = #<_ini[CHANGE_POSITION]Y>
#<chz> = #<_ini[CHANGE_POSITION]Z>

O101 IF [EXISTS[#4920]]
O101 ELSE
  #4920 = #5220                                        (PRINT,save WCS id)
O101 ENDIF

O102 IF [ #<_metric> EQ 1 ]                            (PRINT,set probe feeds according to units )
  #<fast> =  150.0                                     (PRINT,selected speed unit mm/mn )
  #<slow> =   30.0
  #<retract> = 3.0                                     (PRINT,mm )
O102 ELSE
  #<fast> =    6.0                                     (PRINT,selected speed unit in/mn )
  #<slow> =    1.2
  #<retract> = 0.1                                     (PRINT,in )
O102 ENDIF

M9                                                     (PRINT,turn off coolant, will be restored on return if it was on )
M5                                                     (PRINT,turn off spindle, cannot be on during the probe )

M70                                                    (PRINT,save current modal state )
G40                                                    (PRINT,turn cutter radius compensation off here )
G49                                                    (PRINT,clear tool length compensation )
G90                                                    (PRINT,use absolute positioning here )
G94                                                    (PRINT,use feedrate in units/min )

G53 G0 Z[#<trv>]                                       (PRINT,moves to safe height for travelling)
G53 G0 X[#<chx>] Y[#<chy>]
G53 G0 Z[#<chz>]                                       (PRINT,moves to desired potion for manual tool change )
M6                                                     (PRINT,do the normal M6 stuff )

G53 G0 Z[#<trv>]                                       (PRINT,go to high travel level on Z )
#<zDelta>=[#<_z>-#<trv>]                               (PRINT,retrieve absolute offset )
G53 G0 X[#<tsx>] Y[#<tsy>]                             (PRINT,to tool setter )
G38.2 Z[#<_ini[AXIS_Z]MIN_LIMIT>+#<zDelta>] F[#<fast>] (PRINT,trip switch slowly )
G91                                                    (PRINT,relative move )
G0 Z[#<retract>]                                       (PRINT,move away )
G0 Z-[#<retract>/2]                                    (PRINT,go forward )
G38.2 Z-[#<retract>] F[#<slow>]                        (PRINT,trip switch very slowly )
G90                                                    (PRINT,absolute moves )
O103 IF [ #<prevtool> EQ 0 OR #4920 NE #5220 OR #<_current_tool> EQ 99 ] (PRINT,first tool load or WCS change )
  #4921 = #5063                                        (PRINT,save trip point )
  #4920 = #5220                                        (PRINT,save WCS )
  G53 G0 Z[#<trv>]                                     (PRINT,return to safe level )
  M72                                                  (PRINT,restore modal state )
O103 ELSE
  G53 G0 Z[#<trv>]                                     (PRINT,return to high travel level )
  M72                                                  (PRINT,restore modal state )
  G43.1 Z[#5063 - #4921]                               (PRINT,apply offset to Z height after Modal reset )
O103 ENDIF
G0 X[#<xpos>] Y[#<ypos>]                               (PRINT,return to where we were in X=#<xpos> Y=#<ypos> )
                                                       (PRINT,tool change routine END)
O100 ENDIF
O<tool-change> ENDSUB
M2