Hello Matt,
after a G31 you allways should read the exact Position where the probe was hit with:
ZProbePos = GetVar(2002) // 2000 -> X , 2001 -> Y , 2002 -> Z
to write the tool lenght you have to use:
SetToolParam(ToolNr,2, ZProbePos)
to get the current ToolNr:
ToolNr = GetCurrentTool()
Play a Little bit arround with this functions, and you will familar with the complex theme of
offset's and Toollenght.
Thomas