; M6 REMAP Macro for ATC RapidChange rack tool changer
;
; Assumptions
; 1 Tool number is equal to pocket number
; 2 Tool number is the discriminating factor for pick up procedure (from rack, from manual position)
; 3 all tools need to be measured

; Uses tool number mounted as a work around for the broken handler in linuxcnc tooltable
; Note: If #5190 is manually entered into linuxnc.var it will be stored between starts of linux, this is currently not desired.
; Uses M66 E0 (analog input 00 to obtain tool-prep-pocket from iocontrol. This is more stable than a call on #<_hal[iocontrol.0.tool-prep-pocket]> as TxM6 and Tx and M6 seperately are interpreted)

; DustCover actuation is done using a single signal pin via M64 and M65


o<rack_change> sub
    ;(auto-restore modal settings on return)
M73
	; Use Exact Stop mode
G61
    ; print to console (if available)
(print, Storing tool and pocket information in local variables)
#<newtool> = #<selected_tool>
#<oldtool> = #<tool_in_spindle>
    ; call upon iocontrol.0.tool-prep-pocket via M66 and have it stored in #5399
M66 E0
;#<newpocket> = #5399
#5190=#5399
(print, oldtool=#<oldtool>)
(print, newtool=#<newtool>)

    ; we must execute this macro 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
O100 if [#<_task> EQ 0]
        (print, Task is Null)
O100     return [999]
O100 endif

    ; Ensure everything that we do is done in absolute coordinates
G90
    ; Cutter comp off, otherwise G53 might go wrong
G40
    ; Cancel tool offset (not needed until the end)
G49

    ; First things first, rapid to safe Z
G53 G0 Z[#<_ini[AXIS_Z]MAX_LIMIT>-0.1]
(print, Moved safe Z)

    ; First, get rid of the old tool
    ; check if the spindle is not empty by checking if tool_in_spindle GT 0
O200 if [#<oldtool> EQ #<newtool>]
        ; This is a special case in which no tool needs to be dropped off. This assumes that the user has the tool table under control. further down, during pick up of newtool a message will be brought forward to GUI
    (print, do absolutly nothing)
O200 elseif [#<oldtool> GT 0]
    (print, Tool No  #<oldtool> is in the spindle - we need to put it away)
        ; Check if old pocket is within range
    O210 if [#<oldtool> GT 0]
        O220 if [#<oldtool> LE #<_ini[ATC]NUMPOCKETS>]
                ;Tool within range, triggering put move to rack, assuming that the pocket is empty!
            (print, Putting tool away into rack in pocket #<oldtool>)
                ; We are already at safe Z; now rapid to pre position for a specific pocket
            G53 G0 X[#<_ini[ATC]FIRSTPOCKET_X> + [#<oldtool> -1] * #<_ini[ATC]DELTA_X>] Y[#<_ini[ATC]FIRSTPOCKET_Y> + [#<oldtool> -1] * #<_ini[ATC]DELTA_Y>]
                ;Open dust cover
            M65 P0
            G4 P2.0
                ;Drop spindle to IR sensor height
            G53 G0 Z[#<_ini[ATC]OFF_HEIGHT_Z>]
                ;Spin spindle CCW and place tool in rack
            M4 S1200
            G4 P 2.0
            G53 G01 Z #<_ini[ATC]FIRSTPOCKET_Z> F40
                ; Move back to IR sensor height above the old pocket
            G53 G0 Z[#<_ini[ATC]OFF_HEIGHT_Z>]
            M5
            G4 P2.0
                ; After this move there should be no tool left; check pin for going LOW L4
            M66 P1 L0
            O910 IF [#5399 EQ 1]
                (DEBUG, Timeout! Tool still in spindle - Aborting!)
            O910     return [999]
            O910 ENDIF

            (print, Tool released)
        O220 else
                ; The tool's pocket number is not within the range of the rack and the tool needs to be removed manually
                ; Tool is still at safe Z height from before
            (print, Pocket is not within range, moving to manual change positon)
            G53 G0 X #<_ini[ATC]CHANGEX> Y #<_ini[ATC]CHANGEY>
            G53 G0 Z #<_ini[ATC]CHANGEZ>
            ;(MSG, Please remove tool manually from spindle. Afterwards press OK)
                    ; Abuse M6 for a dialog with the user by briefly killing the requested tool and replacing it with T0
            T0
            M6
            T#<newtool>
            (print, Finished  manual drop off)
        o220 endif
    (print,  We are now without a tool at some position either safely above pocket or the manual change position)
    O210 endif
O200 else
        ; There is nothing to do here but end the conditional
    (print, There is no tool in the spindle - No action required at this time)
O200 endif
(print, Reached end of drop off without error)
    ;go to safe Z
G53 G0 Z[#<_ini[AXIS_Z]MAX_LIMIT>-0.1]


    ; Second Step, get new tool
    ; Check if the new tool number is EQ 0
o300 if [#<oldtool> EQ #<newtool>]
        ; This is to check if the requested tool is already in the spindle. If so, all other steps are skipped and a message is displayed to the user using DEBUG
    (DEBUG, Attention - No tool change required. Not doing anything, check if tool offset remains valid)
    M2
O300 elseif [#<newtool> GT 0]
        ; Tool is GT 0 and we thus need to get a tool from somewhere
    (print, New tool: #<newtool> needs to be picked up)
        ; Again, check if #newpocket is within range of the tool rack, otherwise pick at manual change position and measure
    O310 if [#<newtool> GT 0]
            ; Check only for error handling
        O320 if [#<newtool> LE #<_ini[ATC]NUMPOCKETS>]
                ; Tool within rack range, triggering get tool from rack, assuming that the pocket is filled
            (print, Getting tool #<newtool> from rack at pocket #<newtool>)
                ; Assuming that the pockets are populated; rapid  move above selected_pocket at z0
            (print, Moving to safe preposition at z0)
            G53 G0 Z[#<_ini[AXIS_Z]MAX_LIMIT>-0.1]
            G53 G0 X[#<_ini[ATC]FIRSTPOCKET_X> + [#<newtool> -1] * #<_ini[ATC]DELTA_X>] Y[#<_ini[ATC]FIRSTPOCKET_Y> + [#<newtool> -1] * #<_ini[ATC]DELTA_Y>]
               ; Make sure dustcover is open
            M65 P0
            G4 P 2.0
                ; drop Z to IR sensor height
            G53 G0 Z[#<_ini[ATC]OFF_HEIGHT_Z>]
                ; spin spindle CW
            M3 S1200
            G4 P 2.0
            (print, above  pocket #<newtool>, spindle running CW)
                ; move Z to pickup tool
            G53 G01 Z #<_ini[ATC]FIRSTPOCKET_Z> F40
                ; retract Z and repeat pickup to lock spindle nut
            G53 G0 Z[#<_ini[ATC]OFF_HEIGHT_Z>]
            G4 P0.2
            G53 G01 Z #<_ini[ATC]FIRSTPOCKET_Z> F40
            G53 G0 Z[#<_ini[ATC]OFF_HEIGHT_Z>]
            M5
            G4 P2.0
                ; check 2: Is there a tool in the spindle? Check immediately!
            M66 P1 L0
            (print, check tool in spindle AFTER M65 should be 1; VALUE #5399)
            O950 IF [#5399 NE 1]
                (print, Error! No tool found in spindle - Aborting!)
            O950     return [999]
            O950 ENDIF
            (print, Tool picked up, ready to move)
                ; Load tool number
            (print, Moving finished, tool loaded, now tell linuxcnc about the toolchange using M61)
            M61 Q#<newtool>
            G53 G0 Z[#<_ini[AXIS_Z]MAX_LIMIT>-0.1]
            (print, Tool loaded from rack, ready to measure)
                ;Close Dust cover
            M64 P0

        O320 else
                ; The tool's pocket number is not within the range of the rack and the tool needs to picked up at the manual change position
            (print, Selected pocket is not within range, moving to manual pick up)
                ; rapid move to manual pick up position
            G53 G0 Z[#<_ini[AXIS_Z]MAX_LIMIT>-0.1]
            G53 G0 X[#<_ini[ATC]CHANGEX>] Y[#<_ini[ATC]CHANGEY>]
            G53 G0 Z[#<_ini[ATC]CHANGEZ>]
                ;Close dust cover
             M64 P0
                ; using the code being remapped here means 'use builtin behaviour' and should trigger the usual message
            (print, Now trigger M6 to tell the user which tool to pick)
            M6
        O320 ENDIF

            ; Perform tool measurement - first rapid to tool sensor position
        G53 G0 Z[#<_ini[AXIS_Z]MAX_LIMIT>-0.1]
        G53 G0 X[#<_ini[TOOLSENSOR]X>] Y[#<_ini[TOOLSENSOR]Y>]
            ; Turn on airblast to clean tool length sensor
        ;M64 P #<_ini[ATC_PINS]CLEAN_TS>
        G53 G0 Z[#<_ini[TOOLSENSOR]Z>]
            ; Check certain inputs in probe_screen
        O420 if [#<_hal[gmoccapy.searchvel]> LE 0]
            O420 return [-1] ; indicate searchvel <= 0
        O420 endif
        O430 if [#<_hal[gmoccapy.probevel]> LE 0]
            O430 return [-2] ; indicate probevel <= 0
        O430 endif



            ; Perform tool measurement in relative motion
        F #<_hal[gmoccapy.searchvel]>
        G91
        G38.2 Z #<_ini[TOOLSENSOR]MAXPROBE>
        G0 Z0.25
        F #<_hal[gmoccapy.probevel]>
        G38.2 Z-0.5
            ; Turn off airblast to clean sensor
        ;M65 P #<_ini[ATC_PINS]CLEAN_TS>
        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]
        (print, moving away from ts)
        #<touch_result> = #5063
        #<probeheight> = #<_hal[gmoccapy.probeheight]>
        #<blockheight> = #<_hal[gmoccapy.blockheight]>
        (print, #<touch_result>  #<probeheight>  #<blockheight>)
        G10 L1 P#<newtool> Z[#<touch_result> - #<_hal[gmoccapy.probeheight]> + #<_hal[gmoccapy.blockheight]>]
        G43
    O310 endif
O300 endif

(print, End of Program)
o<rack_change> endsub [1]

M2
%