ATC spindle tool offset

G1 X0 Y0 Z0 F4000 - this will crash the tool into the part 5mm. 

My problem is that tool offset is measured coorrectly but it is not applied. 
Initially I thought that maybe something is wrong with my tool change macro but now I am more on the side  of machine config. Because I tried macro manua_change.ngc  without ATC was working fine and now I tried to use it with ATC spindle and tool offset was not applied correctly.. By the way part of the macro for tool measurement is almost the same.  Below you can find part of the code where I measure the tool:
[code]
        G53 G0 Z[#<_ini[TOOLSENSOR]Z>]
        ; Check certain inputs in probe_screen
        O420 if [#<_hal[probe.ps_searchvel]> LE 0]
            O420 return [-1] ; indicate searchvel <= 0 
        O420 endif
        O430 if [#<_hal[probe.ps_probevel]> LE 0]
            O430 return [-2] ; indicate probevel <= 0 
        O430 endif
        M64 P#<_ini[OTHER_PINS]TS>
          ; Perform tool measurement in relative motion
        F #<_hal[probe.ps_searchvel]>
        G91
        G38.2 Z #<_ini[TOOLSENSOR]MAXPROBE>
        G38.4 Z2
        F #<_hal[probe.ps_probevel]>
        G38.2 Z-2
        O440 if [#5070 EQ 0]
            G90
            O440 return [-3] ; indicate probe contact failure to epilog
        O440 endif

        G90
        G53 G0 Z[#<_ini[AXIS_Z]MAX_LIMIT>-0.1]
        G53 G0 Y[#<_ini[TOOLSENSOR]Y> - #<_ini[ATC]SAFE_Y>]
        (print, moving away from ts)
        #<touch_result> = #5063
        #<probeheight> = #<_hal[probe.probeheight]> 
        #<blockheight> = #<_hal[probe.blockheight]>
        (print, #<touch_result>  #<probeheight>  #<blockheight>)
        G10 L1 P#<newtool> Z[#<touch_result> - #<_hal[probe.probeheight]> + #<_hal[probe.blockheight]>]
        G43
    O310 endif
O300 endif
;Turn off tool setter
M65 P#<_ini[OTHER_PINS]TS>