;
;   RosettaCNC (c) 2016-2020
;
; Name              : tool_change.ngc
; Sub UID           : 6
; Version           : 2
; Description       : Tool change macro
; Application       : Standard CNC cycles
; Note              : This macro is called by CNC with M06 or manually with G65 P"tool_change" for optional actions.
;                   : Check the #6000 parameter description for tool change supported types.
;
; DISCLAIMER        : This macro is provided 'AS IS', without WARRANTY of any kind, express or implied.
;                   : The macro is provided as an example only.
;                   : Use this macro at your own risk.
;
; TAKE CARE         : Don't use the CNC #5132..5135 because they are valid ONLY when the macro is called with a M06.
;
; Conventions of input parameters
;
;   Xx              compulsory parameter
;   [Xx]            optional parameter
;   (Xx|Yy|Zz)?     one of compulsory paramaters
;   [Xx|Yy|Zz]*     zero or more optionals parameters
;   [Xx|Yy|Zz]+     at least one or more optionals parameters
;   M!              compulsory parameter don't take care of passed value
;   [M!]            optional parameter don't take care of passed value
;   M[1|2]?         compulsory parameter with one of listed values (e.g. M1 or M2)
;   [M[1|2]?]       optional parameter with one of listed values (e.g. None or M1 or M2)
;
; Format
;
;   M06
;
;   G65 P"tool_change" Aa Bb Cc Ii [Jj] [K!]
;
; Arguments with M6 call
;
;   Aa  #1          tool id of the tool to be loaded
;   Bb  #2          slot of the tool to be loaded
;   Cc  #3          tool id of the tool in use
;   Ii  #4          slot of the tool in use
;
; Extra arguments with G65 call
;
;   Jj  #5          optional action:
;                       #0  = the option is disabled (is the normal condition when the macro is called by a M06)
;                        0  = do not change behavior of tool-setter after a pick in M06 or direct macro call
;                        1  = do not execute tool-setter after a pick
;                        2  = force execute tool-setter after pick
;                        3  = execute only tool-setter of loaded tool
;                        4  = execute only magazine open
;                        5  = execute only magazine close
;                        6  = return magazine state in #<tc.atc.magazine_state>
;                        7  = manually cone unload/load
;   Kk  #6          optional magazine argument:
;                       #0  = the option is disabled (is the normal condition when the macro is called by a M06)
;                       any = don't close magazine, is not the last operation (PICK/PLACE/TOOL-SETTER) to do yet
;
; Protected Parameters
;
;   Param           Description
;   =====           ===========
;   #6000           TOOL_CHANGE [GEN] : Type:
;                                       0   = Dummy tool change:
;                                             This mode only notice to the PLC about the loaded tool ID.
;                                             No other actions or movments will be done.
;                                       1   = Manual Tool Change (MTC) with tool length probing:
;                                             a] Moves to tool-setter position and measures the loaded tool length.
;                                             b] Moves to tool change position and show info about tool to manually change.
;                                             c] Waits for the manual tool change by the operator which confirm that change is done.
;                                             d] Moves to tool-setter position to measures the length of the new tool.
;                                             e] Calcs the difference and uses this value as tool length compensation.
;                                       2   = Manual Tool Change (MTC) with tool length from Tools Library info:
;                                             a] Moves to tool change position and show info about tool to manually change.
;                                             b] Waits for the manual tool change by the operator which confirm that change is done.
;                                             c] If enabled applies the tool length compensation from Tools Libary info.
;                                       10  = Automatic Tool Change (ATC)
;                                             This mode manages a rack of slots using place/side & pick/top approach.
;                                             If there is a loaded tool it will be placed.
;                                             If there is a requested tool it will be picked.
;                                             If the tool length measurement/compensation are enabled they are automaticallly performed.
;                                       11  = Automatic Tool Change (ATC)
;                                             This mode manages a rack of slots using place/top & pick/top approach.
;                                             If there is a loaded tool it will be placed.
;                                             If there is a requested tool it will be picked.
;                                             If the tool length measurement/compensation are enabled they are automaticallly performed.
;   #6001           TOOL_CHANGE [GEN] : Features:
;                                       0   = Back to work position is OFF.
;                                       1   = Back to work position is ON.
;   #6002           TOOL_CHANGE [MTC] : Change position X (MCS)
;   #6003           TOOL_CHANGE [MTC] : Change position Y (MCS)
;   #6004           TOOL_CHANGE [MTC] : Change position Z (MCS)
;   #6005           TOOL_CHANGE [TLS] : Sensor safe position Z (MCS)
;   #6006           TOOL_CHANGE [TLS] : Sensor top/center position X (MCS)
;   #6007           TOOL_CHANGE [TLS] : Sensor top/center position Y (MCS)
;   #6008           TOOL_CHANGE [TLS] : Sensor top/center position Z (MCS)
;   #6009           TOOL_CHANGE [GEN] : Feed during tool change procedure
;   #6010           TOOL_CHANGE [TLS] : Z approaching sensor position (MCS, reached with feed #6011)
;   #6011           TOOL_CHANGE [TLS] : Z feed to sensor approaching (FAST)
;   #6012           TOOL_CHANGE [TLS] : Z feed to 1st probe input capture (SLOW)
;   #6013           TOOL_CHANGE [TLS] : Z feed to 2nd probe input capture (VERY SLOW)
;   #6014           TOOL_CHANGE [TLS] : Z up movement after 1st touch capture (REL)
;   #6015           TOOL_CHANGE [TLS] : Minimum tool lenght accepted
;   #6016           TOOL_CHANGE [TLS] : Enable tool length compensation (only for tool change type 1)
;   #6017           TOOL_CHANGE [TLS] : Probe cleaning output
;   #6018           TOOL_CHANGE [TLS] : Probe cleaning time
;   #6019           TOOL_CHANGE [___] : !!! RESERVED FOR FUTURE USE !!!
;   #6020           TOOL_CHANGE [ATC] : Magazine type:
;                                       0 = none magazine
;                                       1 = magazine with 2 control outputs (open and close with individual pulse output)
;   #6021           TOOL_CHANGE [ATC] : Magazine input open
;   #6022           TOOL_CHANGE [ATC] : Magazine input closed
;   #6023           TOOL_CHANGE [ATC] : Magazine input timeout
;   #6024           TOOL_CHANGE [ATC] : Magazine output time
;   #6025           TOOL_CHANGE [ATC] : Magazine output open
;   #6026           TOOL_CHANGE [ATC] : Magazine output close
;   #6027..#6035    TOOL_CHANGE [___] : !!! RESERVED FOR FUTURE USE !!!
;   #6036           TOOL_CHANGE [ATC] : Fast unload tool holder distance
;   #6037           TOOL_CHANGE [ATC] : Slow unload tool holder distance
;   #6038           TOOL_CHANGE [ATC] : Slow unload tool holder feed
;   #6039           TOOL_CHANGE [ATC] : Fast load tool holder distance
;   #6040           TOOL_CHANGE [ATC] : Medium load tool holder distance
;   #6041           TOOL_CHANGE [ATC] : Medium load tool holder feed
;   #6042           TOOL_CHANGE [ATC] : Final load tool holder feed
;   #6043           TOOL_CHANGE [ATC] : Cone load unload time (used only if "Cone load unload input" is zero)
;   #6044           TOOL_CHANGE [ATC] : Cone load unload input (used only for a NO button and detect the release. You must have a macro that start procedure)
;   #6045           TOOL_CHANGE [ATC] : Cone cleaning blow output
;   #6046           TOOL_CHANGE [ATC] : Bearing cleaning blow output
;   #6047           TOOL_CHANGE [ATC] : Cone grip output
;   #6048           TOOL_CHANGE [ATC] : Cone grip output time
;   #6049           TOOL_CHANGE [ATC] : Slots positions parameters address
;                                       If undefined (#0) or set to 0 the default address is 6061 for maximum 13 slots
;   #6050           TOOL_CHANGE [ATC] : Safe Z
;   #6051           TOOL_CHANGE [ATC] : Unlock input
;   #6052           TOOL_CHANGE [ATC] : Unlock output
;   #6053           TOOL_CHANGE [ATC] : Unlock timeout
;   #6054           TOOL_CHANGE [ATC] : Clamp movement
;   #6055           TOOL_CHANGE [ATC] : Pick distance Z
;   #6056           TOOL_CHANGE [ATC] : Enable tool-setter
;   #6057           TOOL_CHANGE [ATC] : Spindle s1 (tool-holder attached correctly)
;   #6058           TOOL_CHANGE [ATC] : Spindle s2 (collet open)
;   #6059           TOOL_CHANGE [ATC] : Slot axis orientation (0 = along X | 1 = along Y)
;   #6060           TOOL_CHANGE [ATC] : Slots number
;   #6061..#6099    TOOL_CHANGE [ATC] : Slots positions parameters (default, with #6049 = #0 or 0, for 1 to 13 slots) (MCS)
;
O6
    ; === DISCLAIMER ===
    ;
    ; This macro is provided 'AS IS', without WARRANTY of any kind, express or implied.
    ; The macro is provided as an example only.
    ; Use this macro at your own risk.
    ;
    ; NOTE: You can comment line 157 or delete this ADVICE but anyway you will accept the macro DISCLAIMER of the original code.
    ;
    IF [EXISTS[#<tc.disclaimer.showed>] EQ 0] THEN10
        #<tc.disclaimer.showed>=1
        M109 P"<B>DISCLAIMER</B><BR><BR>This macro is provided 'AS IS'</B>,<BR>without WARRANTY of any kind, express or implied.<BR><BR>The macro is provided as an example only.<BR><BR>Use this macro at your own risk." Q2
    END10

    ; store loaded/requested tool/slot ID
    #<tc.requested.tool_id>=#1
    #<tc.requested.slot_id>=#2
    #<tc.loaded.tool_id>=#3
    #<tc.loaded.slot_id>=#4

    ; recover macro optional action when macro is called with G65
    IF [[#5 EQ #0] OR [#5 EQ 0]] THEN10
        #<tc.macro.option>=0
    ELSE10
        #<tc.macro.option>=#5
    END10

    ; recover macro optional magazine argument available when macro called with G65
    IF [EXISTS[#<_args.K>]] THEN10
        #<tc.do_not_close_magazine>=1
    ELSE10
        #<tc.do_not_close_magazine>=0
    END10

    ; initialize ATC parameters
    CALL P"TC_INIT"

    ; store actual PLC states in local variables
    #<_saved.5001>=#5001        ; current TCP position X
    #<_saved.5002>=#5002        ; current TCP position Y
    #<_saved.5003>=#5003        ; current TCP position Z
    #<_saved.5004>=#5004        ; current position A
    #<_saved.5005>=#5005        ; current position B
    #<_saved.5006>=#5006        ; current position C
    #<_saved.5101>=#5101        ; active G-codes - group 01: G00, G01, ...
    #<_saved.5130>=#5130        ; active feed rate (F)
    #<_saved.5151>=#5151        ; active M-codes: M3, M4, M5
    #<_saved.5153>=#5153        ; active M-codes: M7, M9
    #<_saved.5154>=#5154        ; active M-codes: M8, M9
    #<_saved.5103>=#5103        ; active G-codes - group 03: G90, G91

    ; store actual PLC states in global variables
    #<saved.5136>=#5136         ; active feed override mode (P argument of M50)

    ; set absolute coordinate
    G90

    ; check if last G-codes group 01 used instruction was a G00/G01 otherwise, to prevent
    ; an error when setting G#<_saved.5101>, set it to G80 (cancel motion mode)
    IF [[#<_saved.5101> NE 0] AND [#<_saved.5101> NE 1]] THEN #<_saved.5101>=80

    ; check if required only a tool-setter procedure by ATCM macro
    IF [#<tc.macro.option> EQ 3] THEN10
        CALL P"TC_TOUCH_PROBE_EXTERNAL_REQUEST"
        ; enable tool length compensation if enabled
        IF [#<tc.enable_tool_length_compensation> EQ 1] THEN G43 H#<tc.requested.tool_id>
        GOTO10
    ELIF [#<tc.macro.option> EQ 4] THEN10
        CALL P"TC_MAGAZINE_OPEN"
        GOTO10
    ELIF [#<tc.macro.option> EQ 5] THEN10
        #<tc.do_not_close_magazine>=0
        CALL P"TC_MAGAZINE_CLOSE"
        GOTO10
    ELIF [#<tc.macro.option> EQ 6] THEN10
        CALL P"TC_MAGAZINE_UPDATE_STATE"
        GOTO10
    ELIF [#<tc.macro.option> EQ 7] THEN10
        CALL P"TC_TOGGLE_CONE_LOAD_UNLOAD"
        GOTO10
    END10

    ; check if tool change type = 0 then execute sub MANUAL_TOOL_CHANGE_0
    IF [#<tc.type> EQ 0] THEN10
        CALL P"MANUAL_TOOL_CHANGE_0"
        GOTO10
    END10

    ; avoid any activity if tool change type is 10 or 11 and requested tool is same of loaded tool
    IF [[#<tc.type> EQ 10] OR [#<tc.type> EQ 11]] THEN10
        IF [[#<tc.requested.tool_id> EQ 0] AND [#<tc.loaded.tool_id> EQ 0]] THEN M109 P"tool change require a PICK or PLACE or PLACE/PICK action" Q-1
        IF [[#<tc.requested.tool_id> EQ #<tc.loaded.tool_id>]] THEN11
            ; enable tool length compensation if enabled
            IF [#<tc.enable_tool_length_compensation> EQ 1] THEN G43 H#<tc.requested.tool_id>
            GOTO10
        END11
    END10

    ; notice start change procedure to PLC
    M107

    ; show main procedure message for 1s
    M109 P"Tool change procedure" Q4
    G04 P1

    ; set position of axes type "rotary axes for head"
    IF [[#<cnc.a.type>] EQ #<axis.type.rotary_head>] THEN10
        G49
        G53 G01 A0 F#<tc.feed>
    END10
    IF [[#<cnc.b.type>] EQ #<axis.type.rotary_head>] THEN10
        G49
        G53 G01 B0 F#<tc.feed>
    END10
    IF [[#<cnc.c.type>] EQ #<axis.type.rotary_head>] THEN10
        G49
        G53 G01 C0 F#<tc.feed>
    END10

    ; check if Tool change type = 1 the execute sub MANUAL_TOOL_CHANGE_1
    IF [#<tc.type> EQ 1] THEN CALL P"MANUAL_TOOL_CHANGE_1"

    ; check if Tool change type = 2 the execute sub MANUAL_TOOL_CHANGE_2
    IF [#<tc.type> EQ 2] THEN CALL P"MANUAL_TOOL_CHANGE_2"

    ; check if Tool change type = 10 the execute sub AUTOMATIC_TOOL_CHANGE_10
    IF [#<tc.type> EQ 10] THEN CALL P"AUTOMATIC_TOOL_CHANGE_10"

    ; check if Tool change type = 10 the execute sub AUTOMATIC_TOOL_CHANGE_10
    IF [#<tc.type> EQ 11] THEN CALL P"AUTOMATIC_TOOL_CHANGE_11"

    ; move back to saved work position
    IF [#<tc.features> EQ 1] THEN10
        G01 X#<_saved.5001> Y#<_saved.5002>
        G01 Z#<_saved.5003>
        IF [[#<cnc.a.type>] EQ #<axis.type.rotary_head>] THEN G01 A#<_saved.5004>
        IF [[#<cnc.b.type>] EQ #<axis.type.rotary_head>] THEN G01 B#<_saved.5005>
        IF [[#<cnc.c.type>] EQ #<axis.type.rotary_head>] THEN G01 C#<_saved.5005>
    END10

    ( restore saved states )
    
N10
    ; restore spindle & coolants states
    IF [#<_saved.5151> EQ 3] THEN M03
    IF [#<_saved.5151> EQ 4] THEN M04
    IF [#<_saved.5153> EQ 7] THEN M07
    IF [#<_saved.5154> EQ 8] THEN M08

    ; restore save feed
    F#<_saved.5130>

    ; restore g-code group 03 state
    G#<_saved.5103>

    ; restore g-code group 01 state
    G#<_saved.5101>

    ; reset HUD message
    M109 P"" Q4

    ; notice end change procedure to PLC
    M108

M99

; TOOL CHANGE EXIT
;
; Inputs:
;
;   Ee  #8          Exit code
;   Aa  #1          Undefined compulsory protected parameter
;
; Exit codes:
;
;   0       NONE
;   1       Error : One or more compulsory protected parameters are undefined!
;   2       Error : Sensor was found already tripped before to start approach movements to probe!
;   3       Error : Sensor was tripped during approach movements to probe!
;   4       Error : Sensor was found already tripped before to start 1st touch movements!
;   5       Error : Sensor was not found during 1st touch movements!
;   6       Error : Sensor was found already tripped before to start 2nd touch movements!
;   7       Error : Sensor was not found during 2nd touch movements!
;   **      Error : Unknown error<number>!
;
SUB "TC_EXIT"

    ; check for error code
    IF [#<_args.e> EQ 0] THEN M99

    ; reset HUD message
    IF [#<tc.enable_hud> EQ 1] THEN M109 P"" Q4

    ; notice end change procedure to PLC
    M108

    IF [#<_args.e> EQ 1] THEN M109 P"compulsory protected parameter ##<_args.a> is undefined!"   Q-1
    IF [#<_args.e> EQ 2] THEN M109 P"<B>Error during measure of tool length</B><BR><BR>Sensor was found already tripped before to start approach movements to probe!</B><BR>" Q1
    IF [#<_args.e> EQ 3] THEN M109 P"<B>Error during measure of tool length</B><BR><BR>Sensor was tripped during approach movements to probe!</B><BR>" Q1
    IF [#<_args.e> EQ 4] THEN M109 P"<B>Error during measure of tool length</B><BR><BR>Sensor was found already tripped before to start 1st touch movements!</B><BR>" Q1
    IF [#<_args.e> EQ 5] THEN M109 P"<B>Error during measure of tool length</B><BR><BR>Sensor was not found during 1st touch movements!</B><BR>" Q1
    IF [#<_args.e> EQ 6] THEN M109 P"<B>Error during measure of tool length</B><BR><BR>Sensor was found already tripped before to start 2nd touch movements!</B><BR>" Q1
    IF [#<_args.e> EQ 7] THEN M109 P"<B>Error during measure of tool length</B><BR><BR>Sensor was not found during 2nd touch movements!</B><BR>" Q1
    IF [#<_args.e> EQ 8] THEN M109 P"compulsory protected parameter ##<_args.a> has a wrong value!" Q-1

    M109 P"<B>Generic Error<BR><BR>Unknown error #<_args.e>!</B>" Q1

ENDSUB

; MANUAL TOOL CHANGE TYPE 0
;
; This mode only notice to the PLC about the loaded tool ID.
; No other actions or movments will be done.
;
SUB "MANUAL_TOOL_CHANGE_0"

    ; notice to PLC the loaded tool ID
    M61 Q#<tc.requested.tool_id>

ENDSUB

; MANUAL TOOL CHANGE TYPE 1
;
; Manual Tool Change (MTC) with tool length probing:
;   a] Moves to tool-setter position and measures the loaded tool length.
;   b] Moves to tool change position and show info about tool to manually change.
;   c] Waits for the manual tool change by the operator which confirm that change is done.
;   d] Moves to tool-setter position to measures the length of the new tool.
;   e] Calcs the difference and uses this value as tool length compensation.
;
SUB "MANUAL_TOOL_CHANGE_1"

    ; disable spindle, flood & mist
    M05 M09

    ; initialize local variabiles
    #<_measure1_>=0     ; measured tool offset Z

    ; disable tool length compensation to avoid software limits error
    G49

    ; measure of tool to unload : approaching to probe
    IF [#<tc.enable_hud> EQ 1] THEN M109 P"Measure of tool to unload : approacing to probe..." Q4

    ; call touch probe procedure
    CALL P"TC_TOUCH_PROBE" A#<tc.tls.top_center_position.z> B#<tc.tls.1st_capture_feed> C#<tc.tls.up_movement_after_1st_touch> D#<tc.tls.minimum_tool_lenght_accepted> E#<tc.tls.2nd_capture_feed>

    ; calculate the tool length offset as the difference between when the sensor was tripped and the sensor Z position.
    #<_measure1_> = [#5713 - #<tc.tls.top_center_position.z>]

    ; move to sensor safe position Z
    G53 G01 Z#<tc.tls.safe_position.z> F#<tc.feed>

    ; move to tool change position
    G53 G01 Z#<tc.mtc.change_position.z> F#<tc.feed>
    G53 G01 X#<tc.mtc.change_position.x> Y#<tc.mtc.change_position.y>

    ; a different tool should be inserted. Display the message to the user
    M109 P"<BR>Insert tool <B>T#<tc.requested.tool_id></B><BR><@TI=#<tc.requested.tool_id>><BR>" Q2
    G04 P1

    ; notice to PLC the loaded tool ID
    M61 Q#<tc.requested.tool_id>

    ; measure of loaded tool : approaching to probe
    IF [#<tc.enable_hud> EQ 1] THEN M109 P"Measure of loaded tool : approacing to probe..." Q4

    ; call touch probe procedure
    CALL P"TC_TOUCH_PROBE" A#<tc.tls.top_center_position.z> B#<tc.tls.1st_capture_feed> C#<tc.tls.up_movement_after_1st_touch> D#<tc.tls.minimum_tool_lenght_accepted> E#<tc.tls.2nd_capture_feed>

    ; calculate the tool length offset as the difference between when the sensor was tripped and the sensor Z position.
    #<_measure2_> = [#5713 - #<tc.tls.top_center_position.z>]

    ; move to sensor safe position Z
    G53 G01 Z#<tc.tls.safe_position.z> F#<tc.feed>

    ; calc compensation to do
    #<_toll_length_difference_>=[#<_measure2_>-#<_measure1_>+#5422]

    ; enable dynamic tool length compensation (without use of tools library)
    G43.1 Z#<_toll_length_difference_>

ENDSUB

; MANUAL TOOL CHANGE TYPE 2
;
; Manual Tool Change (MTC) with tool length from Tools Library info:
;   a] Moves to tool change position and show info about tool to manually change.
;   b] Waits for the manual tool change by the operator which confirm that change is done.
;   c] If enabled applies the tool length compensation from Tools Libary info.
;
SUB "MANUAL_TOOL_CHANGE_2"

    ; check if a different tool should be inserted.
    IF [#<tc.requested.tool_id> NE #<tc.loaded.tool_id>] THEN10

        ; disable spindle, flood & mist
        M05 M09

        ; disable tool length compensation to avoid software limits error
        G49

        ; move to tool change position
        G53 G01 Z#<tc.mtc.change_position.z> F#<tc.feed>
        G53 G01 X#<tc.mtc.change_position.x> Y#<tc.mtc.change_position.y>

        ; A different tool should be inserted. Display the message to the user.
        M109 P"<BR>Insert tool <B>T#<tc.requested.tool_id></B><BR><@TI=#<tc.requested.tool_id>><BR>" Q2
        G04 P1

        ; notice to PLC the loaded tool ID
        M61 Q#<tc.requested.tool_id>

        ; enable tool length compensation if enabled
        IF [#<tc.enable_tool_length_compensation> EQ 1] THEN G43 H#<tc.requested.tool_id>

    END10

ENDSUB

; AUTOMATIC TOOL CHANGE TYPE 10
;
; This mode manages a rack of slots using place/side & pick/top approach.
; If there is a loaded tool it will be placed.
; If there is a requested tool it will be picked.
; If the tool length measurement/compensation are enabled they are automaticallly performed.
;
SUB "AUTOMATIC_TOOL_CHANGE_10"

    ; checks if slots for the PICK and PLACE actions are valid
    ;
    ; this happen if G-code require a T with a slot higher than available slots to keep it
    IF [[#<tc.loaded.slot_id> LT 0] OR [#<tc.loaded.slot_id> GT #<tc.atc.slots>]] THEN M109 P"tool change require a PLACE slot number not in range (1..#<tc.atc.slots>)" Q-1
    IF [[#<tc.requested.slot_id> LT 0] OR [#<tc.requested.slot_id> GT #<tc.atc.slots>]] THEN M109 P"tool change require a PICK slot number not in range (1..#<tc.atc.slots>)" Q-1

    ; disable tool length compensation to avoid errors in software limits checking
    G49

    ; check actions to do
    #<_has.place>=[#<tc.loaded.tool_id> NE 0]       ; evaluate if tool change has place action
    #<_has.pick>=[#<tc.requested.tool_id> NE 0]     ; evaluate if tool change has pick action

    ( loaded tool placing )

    ; check if necessary to place the loaded tool
    IF [#<_has.place>] THEN10

        ; show place info
        IF [#<tc.enable_hud> EQ 1] THEN M109 P"Tool Changing: PLACE of T#<tc.loaded.tool_id>" Q4

        ; move Z to safe position
        G53 G00 Z#<tc.atc.safe.z>

        ; disable spindle, flood & mist
        M05 M09

        ; open magazine
        CALL P"TC_MAGAZINE_OPEN"

        ; move XY to clamp zone slot of loaded tool to place it
        CALL P"TC_MOVE_XY_TO_CLAMP_ZONE_SLOT" M0

        ; move Z to tool drop position
        G53 G00 Z#<tc.atc.slots[#<tc.loaded.slot_id>].z>

        ; move XY into tool place
        G53 G01 X#<tc.atc.slots[#<tc.loaded.slot_id>].x> Y#<tc.atc.slots[#<tc.loaded.slot_id>].y> F#<tc.feed>

        ; unload tool-holder
        CALL P"TC_TOOLHOLDER_UNLOAD"

        ; inform the PLC that the tool has been dropped
        M61 Q0

        ; move Z to safe position
        G53 G00 Z#<tc.atc.safe.z>

        ; turn OFF tool unlock output
        M63 P#<tc.atc.unlock.output>

        ; check if there is not a pick action
        IF [#<_has.pick> EQ 0] THEN11
            ; move XY to clamp zone slot of just now unloaded tool
            CALL P"TC_MOVE_XY_TO_CLAMP_ZONE_SLOT" M0
        END11

    END10

    ( requested tool picking )

    ; check if necessary to pick the requested tool
    IF [#<_has.pick>] THEN10

        ; show pick info
        IF [#<tc.enable_hud> EQ 1] THEN M109 P"Tool Changing: PICK of T#<tc.requested.tool_id>" Q4

        ; check if place action is enabled
        IF [#<_has.place> EQ 0] THEN11

            ; move Z to safe position
            G53 G00 Z#<tc.atc.safe.z>

            ; disable spindle, flood & mist
            M05 M09

            ; open magazine
            CALL P"TC_MAGAZINE_OPEN"

            ; move XY to outer of tool place position
            G53 G00 X#<tc.atc.slots[#<tc.requested.slot_id>].x> Y#<tc.atc.slots[#<tc.requested.slot_id>].y>

            ; enable bearing and cone cleaning blow
            CALL P"TC_BEARING_CONE_CLEANING_BLOW" E1
            
        ELSE11
            ; move Z to safe position
            G53 G00 Z#<tc.atc.safe.z>

            ; move XY to outer of tool place position
            G53 G00 X#<tc.atc.slots[#<tc.requested.slot_id>].x> Y#<tc.atc.slots[#<tc.requested.slot_id>].y>
        END11

        ; turn ON tool unlock output and wait 1s
        M62 P#<tc.atc.unlock.output>
        G04 P1.0

        ; enable bearing and cone cleaning blow
        CALL P"TC_BEARING_CONE_CLEANING_BLOW" E1

        ; move Z to tool pick position
        #<change.pick.z>=[#<tc.atc.slots[#<tc.requested.slot_id>].z> + #<tc.atc.pick_distance.z>]
        G53 G01 Z#<change.pick.z> F#<tc.feed>

        ; load tool-holder
        CALL P"TC_TOOLHOLDER_LOAD"

        ; disable bearing and cone cleaning blow
        CALL P"TC_BEARING_CONE_CLEANING_BLOW" E0

        ; inform PLC that requested tool has been picked
        M61 Q#<tc.requested.tool_id>

        ; move X or Y to outer of tool place position depending by slot axis orientation
        IF [#<tc.atc.slot.axis.orientation> EQ 0] THEN11
            #<change.outer.y> = [#<tc.atc.slots[#<tc.requested.slot_id>].y> - #<tc.atc.clamp_movement>]
            G53 G00 Y#<change.outer.y>
        ELSE11
            #<change.outer.x> = [#<tc.atc.slots[#<tc.requested.slot_id>].x> - #<tc.atc.clamp_movement>]
            G53 G00 X#<change.outer.x>
        END11

        ; move Z to safe position
        G53 G00 Z#<tc.atc.safe.z>
    END10

    ( new loaded tool measuring and compensation )

    ; check if there was a pick action so a new tool should be in the spindle
    IF [#<_has.pick>] THEN10
    
        ; check if necessary to measure loaded tool
        IF [[[#<tc.atc.enable_tool_setter> EQ 1] AND [#<tc.macro.option> EQ 0]] OR [#<tc.macro.option> EQ 2]] THEN11
            CALL P"TC_TOUCH_PROBE"
        END11

        ; enable tool length compensation if enabled
        IF [#<tc.enable_tool_length_compensation> EQ 1] THEN G43 H#<tc.requested.tool_id>
        
    END10

    ; close magazine
    CALL P"TC_MAGAZINE_CLOSE"

ENDSUB

; AUTOMATIC TOOL CHANGE TYPE 11
;
; This mode manages a rack of slots using place/top & pick/top approach.
; If there is a loaded tool it will be placed.
; If there is a requested tool it will be picked.
; If the tool length measurement/compensation are enabled they are automaticallly performed.
;
SUB "AUTOMATIC_TOOL_CHANGE_11"

    ; checks if slots for the PICK and PLACE actions are valid
    ;
    ; this happen if G-code require a T with a slot higher than available slots to keep it
    IF [[#<tc.loaded.slot_id> LT 0] OR [#<tc.loaded.slot_id> GT #<tc.atc.slots>]] THEN M109 P"tool change require a PLACE slot number not in range (1..#<tc.atc.slots>)" Q-1
    IF [[#<tc.requested.slot_id> LT 0] OR [#<tc.requested.slot_id> GT #<tc.atc.slots>]] THEN M109 P"tool change require a PICK slot number not in range (1..#<tc.atc.slots>)" Q-1

    ; disable tool length compensation to avoid errors in software limits checking
    G49

    ; check actions to do
    #<_has.place>=[#<tc.loaded.tool_id> NE 0]       ; evaluate if tool change has place action
    #<_has.pick>=[#<tc.requested.tool_id> NE 0]     ; evaluate if tool change has pick action

    ( loaded tool placing )

    ; check if necessary to place the loaded tool
    IF [#<_has.place>] THEN10

        ; show place info
        IF [#<tc.enable_hud> EQ 1] THEN M109 P"Tool Changing: PLACE of T#<tc.loaded.tool_id>" Q4

        ; move Z to safe position
        G53 G00 Z#<tc.atc.safe.z>

        ; disable spindle, flood & mist
        M05 M09

        ; open magazine
        CALL P"TC_MAGAZINE_OPEN"

        ; move XY to clamp zone slot of loaded tool to place it
        CALL P"TC_MOVE_XY_TO_CLAMP_ZONE_SLOT" M0

        ; move XY into tool place
        G53 G01 X#<tc.atc.slots[#<tc.loaded.slot_id>].x> Y#<tc.atc.slots[#<tc.loaded.slot_id>].y> F#<tc.feed>

        ; aproach to tool-holder drop zone
        G53 G01 Z[#<tc.atc.slots[#<tc.loaded.slot_id>].z> + #<tc.atc.fast.unload.tool.holder.distance>] F#<tc.feed>
        G53 G01 Z[#<tc.atc.slots[#<tc.loaded.slot_id>].z> + #<tc.atc.slow.unload.tool.holder.distance>] F#<tc.atc.slow.unload.tool.holder.feed>

        ; unload tool-holder
        CALL P"TC_TOOLHOLDER_UNLOAD"

        ; inform the PLC that the tool has been dropped
        M61 Q0

        ; move Z to safe position
        G53 G00 Z#<tc.atc.safe.z>

        ; turn OFF tool unlock output
        M63 P#<tc.atc.unlock.output>

        ; check if there is not a pick action
        IF [#<_has.pick> EQ 0] THEN11
            ; move XY to clamp zone slot of just now unloaded tool
            CALL P"TC_MOVE_XY_TO_CLAMP_ZONE_SLOT" M0
        END11

    END10

    ( requested tool picking )

    ; check if necessary to pick the requested tool
    IF [#<_has.pick>] THEN10

        ; show pick info
        IF [#<tc.enable_hud> EQ 1] THEN M109 P"Tool Changing: PICK of T#<tc.requested.tool_id>" Q4

        ; check if place action is enabled
        IF [#<_has.place> EQ 0] THEN11

            ; move Z to safe position
            G53 G00 Z#<tc.atc.safe.z>

            ; disable spindle, flood & mist
            M05 M09

            ; open magazine
            CALL P"TC_MAGAZINE_OPEN"

            ; move XY to clamp zone slot of requested tool to pick it
            CALL P"TC_MOVE_XY_TO_CLAMP_ZONE_SLOT" M1

            ; move XY into tool place
            G53 G01 X#<tc.atc.slots[#<tc.requested.slot_id>].x> Y#<tc.atc.slots[#<tc.requested.slot_id>].y> F#<tc.feed>
        ELSE11
            ; move XY to outer of tool place position
            G53 G00 X#<tc.atc.slots[#<tc.requested.slot_id>].x> Y#<tc.atc.slots[#<tc.requested.slot_id>].y>
        END11

        ( BEGIN UNLOAD TOOL-HOLDER )

        ; aproach to tool-holder drop zone
        G53 G01 Z[#<tc.atc.slots[#<tc.requested.slot_id>].z> + #<tc.atc.fast.load.tool.holder.distance>] F#<tc.feed>

        ; turn ON tool unlock output
        M62 P#<tc.atc.unlock.output>

        ; enable bearing and cone cleaning blow
        CALL P"TC_BEARING_CONE_CLEANING_BLOW" E1

        ; check if required unlock output feedback
        IF [#<tc.atc.unlock.input> GT 0] THEN11
            ; wait and check for tool unlock input signal with timeout
            M66 P#<tc.atc.unlock.input> L#<wait_input.high> Q#<tc.atc.unlock.timeout>
            IF [#5722 EQ #<wait_input.failure>] THEN M109 P"<B>TOOL T#<tc.requested.tool_id> PICK ERROR</B><BR><BR>Collet Unlock Input is LOW<BR>" Q1
        END11

        ; wait and check spindle S1/S2 for S1 HIGH & S2 LOW
        CALL P"TC_WAIT_AND_CHECK_SPINDLE_S1_S2_FOR_HL"

        ; aproach to tool-holder drop zone
        G53 G01 Z[#<tc.atc.slots[#<tc.requested.slot_id>].z> + #<tc.atc.medium.load.tool.holder.distance>] F#<tc.atc.medium.load.tool.holder.feed>

        ; disable bearing and cone cleaning blow
        CALL P"TC_BEARING_CONE_CLEANING_BLOW" E0

        ; move Z to tool pick position
        G53 G01 Z#<tc.atc.slots[#<tc.requested.slot_id>].z> F#<tc.atc.final.load.tool.holder.feed>

        ; turn OFF tool unlock output and wait 1s
        M63 P#<tc.atc.unlock.output>

        ; check if required unlock output feedback
        IF [#<tc.atc.unlock.input> GT 0] THEN11

            ; wait settle time for P5 input stabilizatin
            G04 P1.0

            ; wait and check for tool unlock input signal with timeout
            M66 P#<tc.atc.unlock.input> L#<wait_input.high> Q#<tc.atc.unlock.timeout>
            IF [#5722 EQ #<wait_input.failure>] THEN12
                M109 P"<B>TOOL T#<tc.requested.tool_id> PICK ERROR</B><BR><BR>Cone present Input is LOW<BR>" Q1
            END12
        END11

        ; cone grip pulse management
        CALL P"TC_CONE_GRIP_PULSE"

        ; inform PLC that requested tool has been picked
        M61 Q#<tc.requested.tool_id>

        ; move Z to safe position
        G53 G01 Z#<tc.atc.safe.z> F#<tc.feed>

        ; move XY to clamp zone slot of requested tool just now loaded
        CALL P"TC_MOVE_XY_TO_CLAMP_ZONE_SLOT" M1

    END10

    ( new loaded tool measuring and compensation )

    ; check if there was a pick action so a new tool should be in the spindle
    IF [#<_has.pick>] THEN10
    
        ; check if necessary to measure loaded tool
        IF [[[#<tc.atc.enable_tool_setter> EQ 1] AND [#<tc.macro.option> EQ 0]] OR [#<tc.macro.option> EQ 2]] THEN11
            CALL P"TC_TOUCH_PROBE"
        END11

        ; enable tool length compensation if enabled
        IF [#<tc.enable_tool_length_compensation> EQ 1] THEN G43 H#<tc.requested.tool_id>
        
    END10

    ; close magazine
    CALL P"TC_MAGAZINE_CLOSE"

ENDSUB

;   TOOL CHANGE CONE GRIP PULSE
;
SUB "TC_CONE_GRIP_PULSE"

    ; check if required cone grip pulse
    IF [#<tc.atc.cone.grip.output> GT 0] THEN10
        G04 P#<tc.atc.cone.grip.output.time>
        M62 P#<tc.atc.cone.grip.output>
        G04 P#<tc.atc.cone.grip.output.time>
        M63 P#<tc.atc.cone.grip.output>
    END10

ENDSUB

;   TOOL CHANGE BEARING/CONE CLEANING BLOW
;
;   Arguments
;       E0  #8      Enabled state: OFF
;       E1  #8      Enabled state: ON
;
SUB "TC_BEARING_CONE_CLEANING_BLOW"

    ; evaluate enabled state
    IF [#8 EQ 0] THEN10

        ; check if required cone clearing blow and put it OFF
        IF [#<tc.atc.cone.cleaning.blow.output> GT 0] THEN11
            M63 P#<tc.atc.cone.cleaning.blow.output>
        END11
        
        ; check if required bearing cleaning blow and put it OFF
        IF [#<tc.atc.bearing.cleaning.blow.output> GT 0] THEN11
            M63 P#<tc.atc.bearing.cleaning.blow.output>
        END11
    
    ELSE10

        ; check if required cone clearing blow and put it ON
        IF [#<tc.atc.cone.cleaning.blow.output> GT 0] THEN11
            M62 P#<tc.atc.cone.cleaning.blow.output>
        END11

        ; check if required bearing cleaning blow and put it ON
        IF [#<tc.atc.bearing.cleaning.blow.output> GT 0] THEN11
            M62 P#<tc.atc.bearing.cleaning.blow.output>
        END11
    
    END10

ENDSUB

;   TOOL CHANGE WAIT AND CHECK SPINDLE S1/S2 FOR S1 LOW & S2 HIGH 
;
SUB "TC_WAIT_AND_CHECK_SPINDLE_S1_S2_FOR_LH"

    ; check if required spindle S1/S2 management
    IF [[#<tc.atc.spindle.s1> GT 0] AND [#<tc.atc.spindle.s2> GT 0]] THEN10
        ; wait and check for spindle S1/S2 management
        M66 P#<tc.atc.spindle.s1> L#<wait_input.low> Q#<tc.atc.unlock.timeout>
        IF [#5722 EQ #<wait_input.failure>] THEN M109 P"<B>TOOL T#<tc.loaded.tool_id> PLACE ERROR</B><BR><BR>Spindle S1 is HIGH<BR>" Q1
        M66 P#<tc.atc.spindle.s2> L#<wait_input.high> Q#<tc.atc.unlock.timeout>
        IF [#5722 EQ #<wait_input.failure>] THEN M109 P"<B>TOOL T#<tc.loaded.tool_id> PLACE ERROR</B><BR><BR>Spindle S2 is LOW<BR>" Q1
    END10

ENDSUB

;   TOOL CHANGE WAIT AND CHECK SPINDLE S1/S2 FOR S1 HIGH & S2 LOW
;
SUB "TC_WAIT_AND_CHECK_SPINDLE_S1_S2_FOR_HL"

    ; check if required spindle S1/S2 management
    IF [[#<tc.atc.spindle.s1> GT 0] AND [#<tc.atc.spindle.s2> GT 0]] THEN10
        ; wait and check for spindle S1/S2 management
        M66 P#<tc.atc.spindle.s1> L#<wait_input.high> Q#<tc.atc.unlock.timeout>
        IF [#5722 EQ #<wait_input.failure>] THEN M109 P"<B>TOOL T#<tc.loaded.tool_id> PICK ERROR</B><BR><BR>Spindle S1 is LOW<BR>" Q1
        M66 P#<tc.atc.spindle.s2> L#<wait_input.low> Q#<tc.atc.unlock.timeout>
        IF [#5722 EQ #<wait_input.failure>] THEN M109 P"<B>TOOL T#<tc.loaded.tool_id> PICK ERROR</B><BR><BR>Spindle S2 is HIGH<BR>" Q1
    END10

ENDSUB

;   TOOL CHANGE MOVE XY TO CLAMP ZONE SLOT FOR LOADED OR SELECTED TOOL
;
;   Arguments
;       M0  #13     Required position: for the loaded tool
;       M1  #13     Required position: for the requested tool
;
SUB "TC_MOVE_XY_TO_CLAMP_ZONE_SLOT"

    ; evaluate required position
    IF [#13 EQ 0] THEN10
    
        ; move XY to outer of tool place position depending by slot axis orientation
        IF [#<tc.atc.slot.axis.orientation> EQ 0] THEN11
            G53 G00 Y[#<tc.atc.slots[#<tc.loaded.slot_id>].y> - #<tc.atc.clamp_movement>]
            G53 G00 X#<tc.atc.slots[#<tc.loaded.slot_id>].x>
        ELSE11
            G53 G00 X[#<tc.atc.slots[#<tc.loaded.slot_id>].x> - #<tc.atc.clamp_movement>]
            G53 G00 Y#<tc.atc.slots[#<tc.loaded.slot_id>].y>
        END11
    
    ELSE10

        ; move XY to outer of tool place position depending by slot axis orientation
        IF [#<tc.atc.slot.axis.orientation> EQ 0] THEN11
            G53 G00 Y[#<tc.atc.slots[#<tc.requested.slot_id>].y> - #<tc.atc.clamp_movement>]
            G53 G00 X#<tc.atc.slots[#<tc.requested.slot_id>].x>
        ELSE11
            G53 G00 X[#<tc.atc.slots[#<tc.requested.slot_id>].x> - #<tc.atc.clamp_movement>]
            G53 G00 Y#<tc.atc.slots[#<tc.requested.slot_id>].y>
        END11
    
    END10

ENDSUB

;   TOOL CHANGE MOVE TO SAFE Z CLAMP ZONE TOOL-SETTER
;
SUB "TC_MOVE_TO_SAFE_Z_CLAMP_ZONE_TOOLSETTER"

    ; move Z to safe position
    G53 G00 Z#<tc.tls.safe_position.z>

    ; move XY to outer of tool-setter position depending by slot axis orientation
    IF [#<tc.atc.slot.axis.orientation> EQ 0] THEN11
        G53 G00 Y[#<tc.tls.top_center_position.y> - #<tc.atc.clamp_movement>]
        G53 G00 X#<tc.tls.top_center_position.x>
    ELSE11
        G53 G00 X[#<tc.tls.top_center_position.x> - #<tc.atc.clamp_movement>]
        G53 G00 Y#<tc.tls.top_center_position.y>
    END11

ENDSUB

;   TOOL CHANGE TOUCH PROBE EXTERNAL REQUEST
;
SUB "TC_TOUCH_PROBE_EXTERNAL_REQUEST"

    ; disable tool length compensation to avoid software limits error
    G49

    ; move to safe Z clamp zone tool-setter
    CALL P"TC_MOVE_TO_SAFE_Z_CLAMP_ZONE_TOOLSETTER"

    ; open magazine
    CALL P"TC_MAGAZINE_OPEN"

    ; call touch probe procedure
    CALL P"TC_TOUCH_PROBE"

    ; move to safe Z clamp zone tool-setter
    CALL P"TC_MOVE_TO_SAFE_Z_CLAMP_ZONE_TOOLSETTER"

    ; close magazine
    CALL P"TC_MAGAZINE_CLOSE"

ENDSUB

;   TOOL CHANGE TOUCH PROBE
;
SUB "TC_TOUCH_PROBE"

    ; check mode type
    IF [[#<tc.type> EQ 10] OR [#<tc.type> EQ 11]] THEN10
        ; move to safe Z clamp zone tool-setter
        CALL P"TC_MOVE_TO_SAFE_Z_CLAMP_ZONE_TOOLSETTER"
    ELSE10
        ; move to safe Z zone tool-setter
        G53 G01 Z#<tc.tls.safe_position.z> F#<tc.feed>
    END10

    ; move XY to probe positions
    G53 G00 X#<tc.tls.top_center_position.x> Y#<tc.tls.top_center_position.y>

    ; check if required cleaning probe pulse
    IF [#<tc.tls.probe.cleaning.output> GT 0] THEN11
        M62 P#<tc.tls.probe.cleaning.output>
        G04 P#<tc.tls.probe.cleaning.output.time>
        M63 P#<tc.tls.probe.cleaning.output>
    END11

    ; move z to probe approaching position
    G53 G38.3 Z#<tc.tls.approaching_position.z> X#<tc.tls.top_center_position.x> Y#<tc.tls.top_center_position.y> F#<tc.tls.approaching_feed>

    ; check if sensor was found already tripped before to start approach movements to probe
    IF [#5700 EQ -2] THEN CALL P"TC_EXIT" E2

    ; check if sensor was tripped during approach movements to probe
    IF [#5700 EQ 1] THEN CALL P"TC_EXIT" E3

    ; call tc touch probe managemnt
    CALL P"TC_TOUCH_PROBE_MAIN" A#<tc.tls.top_center_position.z> B#<tc.tls.1st_capture_feed> C#<tc.tls.up_movement_after_1st_touch> D#<tc.tls.minimum_tool_lenght_accepted> E#<tc.tls.2nd_capture_feed>

    ; calc and set new tool length (offset Z)
    #<_offset.z_>=[#5713 - #<tc.tls.top_center_position.z>]
    G10 L1 P#<tc.requested.tool_id> V#<tc.requested.slot_id> X0 Y0 Z#<_offset.z_>

    ; show calculated tool length
    IF [#<tc.enable_hud> EQ 1] THEN M109 P"Tool T#<tc.requested.tool_id> measured length = #<_offset.z_>mm" Q4

    ; check mode type
    IF [[#<tc.type> EQ 10] OR [#<tc.type> EQ 11]] THEN10
        ; move to safe Z clamp zone tool-setter
        CALL P"TC_MOVE_TO_SAFE_Z_CLAMP_ZONE_TOOLSETTER"
    ELSE10
        ; move to safe Z zone tool-setter
        G53 G01 Z#<tc.tls.safe_position.z> F#<tc.feed>
    END10

ENDSUB

; TOOL CHANGE TOUCH PROBE MAIN
;
; Inputs:
;
;   Ee  #8          Exit code
;   Aa  #1          Sensor top/center position Z (MCS)
;   Bb  #2          Z feed to 1st probe input capture (SLOW)
;   Cc  #3          Z up movement after 1st touch capture (REL)
;   Dd  #7          Minimum tool lenght accepted
;   Ee  #8          Z feed to 2nd probe input capture (VERY SLOW)
;
SUB "TC_TOUCH_PROBE_MAIN"

    ; disable the feed rate override controls
    ;
    ; NOTE
    ; ====
    ; During debug movements, for safety, you can disable this code and use feed rate OVR
    M50 P0

    ; measure of tool : 1st touch
    IF [#<tc.enable_hud> EQ 1] THEN M109 P"Tool Measuring: Approach for 1st touch..." Q4
    G53 G38.3 Z[#<_args.a>+#<_args.d>] F#<_args.b>

    ; check if sensor was found already tripped before to start 1st touch movements
    IF [#5700 EQ -2] THEN10
        ; restore the feed rate override controls
        M50 P#<saved.5136>

        ; move to safe Z clamp zone tool-setter and notice error
        CALL P"TC_MOVE_TO_SAFE_Z_CLAMP_ZONE_TOOLSETTER"
        CALL P"TC_EXIT" E4
    END10

    ; check if sensor was not found during 1st touch movements
    IF [#5700 NE 1] THEN10
        ; restore the feed rate override controls
        M50 P#<saved.5136>

        ; move to safe Z clamp zone tool-setter and notice error
        CALL P"TC_MOVE_TO_SAFE_Z_CLAMP_ZONE_TOOLSETTER"
        CALL P"TC_EXIT" E5
    END10

    ; measure of tool : 2nd touch
    IF [#<tc.enable_hud> EQ 1] THEN M109 P"Tool Measuring: Approach for 2nd touch..." Q4

    G53 G00 Z[#5713+#<_args.c>]
    G53 G38.3 Z[#<_args.a>+#<_args.d>] F#<_args.e>

    ; check if sensor was found already tripped before to start 2nd touch movements
    IF [#5700 EQ -2] THEN10
        ; restore the feed rate override controls
        M50 P#<saved.5136>

        ; move to safe Z clamp zone tool-setter and notice error
        CALL P"TC_MOVE_TO_SAFE_Z_CLAMP_ZONE_TOOLSETTER"
        CALL P"TC_EXIT" E6
    END10

    ; check if sensor was not found during 2nd touch movements
    IF [#5700 NE 1] THEN10
        ; restore the feed rate override controls
        M50 P#<saved.5136>

        ; move to safe Z clamp zone tool-setter and notice error
        CALL P"TC_MOVE_TO_SAFE_Z_CLAMP_ZONE_TOOLSETTER"
        CALL P"TC_EXIT" E7
    END10

    ; restore the feed rate override controls
    M50 P#<saved.5136>

ENDSUB

;   TOOL CHANGE TOOLHOLDER LOAD (ATC)
;
SUB "TC_TOOLHOLDER_LOAD"

    ; turn OFF tool unlock output and wait 1s
    M63 P#<tc.atc.unlock.output>
    G04 P1.0

    ; check if required unlock output feedback
    IF [#<tc.atc.unlock.input> GT 0] THEN10
        ; wait and check for tool unlock input signal with timeout
        M66 P#<tc.atc.unlock.input> L#<wait_input.high> Q#<tc.atc.unlock.timeout>
        IF [#5722 EQ #<wait_input.failure>] THEN M109 P"<B>TOOL T#<tc.requested.tool_id> PICK ERROR</B><BR><BR>Collet Unlock Input is LOW<BR>" Q1
    END10

    ; wait and check spindle S1/S2 for S1 HIGH & S2 LOW
    CALL P"TC_WAIT_AND_CHECK_SPINDLE_S1_S2_FOR_HL"

M99

;   TOOL CHANGE TOOLHOLDER UNLOAD (ATC)
;
SUB "TC_TOOLHOLDER_UNLOAD"

    ; turn ON tool unlock output and wait 1s
    M62 P#<tc.atc.unlock.output>
    G04 P1.0

    ; check if required unlock output feedback
    IF [#<tc.atc.unlock.input> GT 0] THEN10
        ; wait and check for tool unlock input signal with timeout
        M66 P#<tc.atc.unlock.input> L#<wait_input.low> Q#<tc.atc.unlock.timeout>
        IF [#5722 EQ #<wait_input.failure>] THEN11
            ; turn OFF tool unlock output
            M63 P#<tc.atc.unlock.output>
            ; notice error and stop
            M109 P"<B>TOOL T#<tc.requested.tool_id> PLACE ERROR</B><BR><BR>Collet Unlock Input is HIGH<BR>" Q1
        END11
    END10

    ; wait and check spindle S1/S2 for S1 LOW & S2 HIGH
    CALL P"TC_WAIT_AND_CHECK_SPINDLE_S1_S2_FOR_LH"

ENDSUB

;   TOOL CHANGE MAGAZINE UPDATE STATE
;
;   #<tc.atc.magazine_state>
;   0 = undefined
;   1 = open
;   2 = closed
;
SUB "TC_MAGAZINE_UPDATE_STATE"

    ; set magazine state to UNDEFINED
    #<tc.atc.magazine_state>=0

    ; evaluate magazine type
    IF [#<tc.atc.magazine.type> EQ 1] THEN10

        IF [[#<tc.atc.magazine.input.open> GT 0] AND [#<tc.atc.magazine.input.closed> GT 0]] THEN11

            M66 P#<tc.atc.magazine.input.open> L#<wait_input.immediate>
            IF [#5722 EQ #<wait_input.failure>] THEN M109 P"Error in magazine input recovering states" Q1
            #<_open_>=#5720
            M66 P#<tc.atc.magazine.input.closed> L#<wait_input.immediate>
            IF [#5722 EQ #<wait_input.failure>] THEN M109 P"Error in magazine input recovering states" Q1
            #<_closed_>=#5720
            IF [[#<_open_> EQ 1] AND [#<_closed_> EQ 0]] THEN #<tc.atc.magazine_state>=1
            IF [[#<_open_> EQ 0] AND [#<_closed_> EQ 1]] THEN #<tc.atc.magazine_state>=2

        END11

    END10

ENDSUB

;   TOOL CHANGE MAGAZINE OPEN
;
SUB "TC_MAGAZINE_OPEN"

    ; get magazine state
    CALL P"TC_MAGAZINE_UPDATE_STATE"

    ; evaluate magazine type
    IF [#<tc.atc.magazine.type> EQ 1] THEN10

        ; if magazine is already OPEN nothing to do
        IF [#<tc.atc.magazine_state> EQ 1] THEN11
            M99
        END11

        ; open magazine
        IF [#<tc.enable_hud> EQ 1] THEN M109 P"Opening Magazine" Q4
        #<tc.atc.magazine_state>=0
        M63 P#<tc.atc.magazine.output.close>
        M62 P#<tc.atc.magazine.output.open>
        IF [#<tc.atc.magazine.input.open> GT 0] THEN11
            M66 P#<tc.atc.magazine.input.open> L#<wait_input.high> Q#<tc.atc.magazine.input.timeout>
            M63 P#<tc.atc.magazine.output.open>
            IF [#5722 EQ #<wait_input.failure>] THEN M109 P"Magazine not reached open state<BR>in the specified time" Q1
        ELSE11
            G4 P#<tc.atc.magazine.output.time>
            M63 P#<tc.atc.magazine.output.open>
        END11
        IF [#<tc.atc.magazine.input.closed> GT 0] THEN11
            M66 P#<tc.atc.magazine.input.closed> L#<wait_input.immediate>
            IF [#5722 EQ #<wait_input.failure>] THEN M109 P"Error in magazine input recovering states" Q1
            IF [#5720 EQ 1] THEN M109 P"Magazine in wrong state<BR><BR>Opened/Closed inputs are HIGH" Q1
        END11
        #<tc.atc.magazine_state>=1
        IF [#<tc.enable_hud> EQ 1] THEN M109 P"" Q4

    END10

ENDSUB

;   TOOL CHANGE MAGAZINE CLOSE
;
SUB "TC_MAGAZINE_CLOSE"

    ; check if magazine close is bypassed
    IF [#<tc.do_not_close_magazine> EQ 1] THEN M99

    ; get magazine state
    CALL P"TC_MAGAZINE_UPDATE_STATE"

    ; evaluate magazine type
    IF [#<tc.atc.magazine.type> EQ 1] THEN10

        ; if magazine is already CLOSED nothing to do
        IF [#<tc.atc.magazine_state> EQ 2] THEN11
            M99
        END11

        ; close magazine
        IF [#<tc.enable_hud> EQ 1] THEN M109 P"Closing Magazine" Q4
        #<tc.atc.magazine_state>=0
        M63 P#<tc.atc.magazine.output.open>
        M62 P#<tc.atc.magazine.output.close>
        IF [#<tc.atc.magazine.input.closed> GT 0] THEN11
            M66 P#<tc.atc.magazine.input.closed> L#<wait_input.high> Q#<tc.atc.magazine.input.timeout>
            M63 P#<tc.atc.magazine.output.close>
            IF [#5722 EQ #<wait_input.failure>] THEN M109 P"Magazine not reached closed state<BR>in the specified time" Q1
        ELSE11
            G4 P#<tc.atc.magazine.output.time>
            M63 P#<tc.atc.magazine.output.close>
        END11
        IF [#<tc.atc.magazine.input.open> GT 0] THEN11
            M66 P#<tc.atc.magazine.input.open> L#<wait_input.immediate>
            IF [#5722 EQ #<wait_input.failure>] THEN M109 P"Error in magazine input recovering states" Q1
            IF [#5720 EQ 1] THEN M109 P"Magazine in wrong state<BR><BR>Opened/Closed inputs are HIGH" Q1
        END11
        #<tc.atc.magazine_state>=2
        IF [#<tc.enable_hud> EQ 1] THEN M109 P"" Q4

    END10

ENDSUB

; TOOL CHANGE TOGGLE CONE LOAD/UNLOAD
;
SUB "TC_TOGGLE_CONE_LOAD_UNLOAD"

    ; turn ON tool unlock output and wait 1s
    M62 P#<tc.atc.unlock.output>
    G04 P1.0

    ; wait cone load/unload input goes low
    IF [#<tc.atc.cone.load.unload.input> GT 0] THEN11
        M66 P#<tc.atc.cone.load.unload.input> L#<wait_input.low>
        IF [#5722 EQ #<wait_input.failure>] THEN M109 P"Unable to get cone load/unload input state" Q1
    ELSE11
        G04 P#<tc.atc.cone.load.unload.time>
    END11

    ; turn OFF tool unlock output
    M63 P#<tc.atc.unlock.output>

    ; check if required cone grip pulse
    IF [#<tc.atc.cone.grip.output> GT 0] THEN11
        G04 P#<tc.atc.cone.grip.output.time>
        M62 P#<tc.atc.cone.grip.output>
        G04 P#<tc.atc.cone.grip.output.time>
        M63 P#<tc.atc.cone.grip.output>
    END11

ENDSUB

; TOOL CHANGE INITIALIZATION
;
SUB "TC_INIT"
    ; if true enable all hud messages
    #<tc.enable_hud> = 0

    ; check if there are compulsory protected parameters undefined

    ; tool change generic parameters
    IF [#6000 EQ #0] THEN CALL P"TC_EXIT" E1 A6000
    IF [#6001 EQ #0] THEN CALL P"TC_EXIT" E1 A6001
    IF [#6009 EQ #0] THEN CALL P"TC_EXIT" E1 A6009
    IF [#6016 EQ #0] THEN CALL P"TC_EXIT" E1 A6016
    ; manual tool change parameters
    IF [#6002 EQ #0] THEN CALL P"TC_EXIT" E1 A6002
    IF [#6003 EQ #0] THEN CALL P"TC_EXIT" E1 A6003
    IF [#6004 EQ #0] THEN CALL P"TC_EXIT" E1 A6004
    ; tls (tool length presetter) parameters
    IF [#6005 EQ #0] THEN CALL P"TC_EXIT" E1 A6005
    IF [#6006 EQ #0] THEN CALL P"TC_EXIT" E1 A6006
    IF [#6007 EQ #0] THEN CALL P"TC_EXIT" E1 A6007
    IF [#6008 EQ #0] THEN CALL P"TC_EXIT" E1 A6008
    IF [#6010 EQ #0] THEN CALL P"TC_EXIT" E1 A6010
    IF [#6011 EQ #0] THEN CALL P"TC_EXIT" E1 A6011
    IF [#6012 EQ #0] THEN CALL P"TC_EXIT" E1 A6012
    IF [#6013 EQ #0] THEN CALL P"TC_EXIT" E1 A6013
    IF [#6014 EQ #0] THEN CALL P"TC_EXIT" E1 A6014
    IF [#6015 EQ #0] THEN CALL P"TC_EXIT" E1 A6015
    IF [#6017 EQ #0] THEN CALL P"TC_EXIT" E1 A6017
    IF [#6018 EQ #0] THEN CALL P"TC_EXIT" E1 A6018
    ; atc parameters
    IF [#6020 EQ #0] THEN CALL P"TC_EXIT" E1 A6020
    IF [#6021 EQ #0] THEN CALL P"TC_EXIT" E1 A6021
    IF [#6022 EQ #0] THEN CALL P"TC_EXIT" E1 A6022
    IF [#6023 EQ #0] THEN CALL P"TC_EXIT" E1 A6023
    IF [#6024 EQ #0] THEN CALL P"TC_EXIT" E1 A6024
    IF [#6025 EQ #0] THEN CALL P"TC_EXIT" E1 A6025
    IF [#6026 EQ #0] THEN CALL P"TC_EXIT" E1 A6026
    IF [#6036 EQ #0] THEN CALL P"TC_EXIT" E1 A6036
    IF [#6037 EQ #0] THEN CALL P"TC_EXIT" E1 A6037
    IF [#6038 EQ #0] THEN CALL P"TC_EXIT" E1 A6038
    IF [#6039 EQ #0] THEN CALL P"TC_EXIT" E1 A6039
    IF [#6040 EQ #0] THEN CALL P"TC_EXIT" E1 A6040
    IF [#6041 EQ #0] THEN CALL P"TC_EXIT" E1 A6041
    IF [#6042 EQ #0] THEN CALL P"TC_EXIT" E1 A6042
    IF [#6043 EQ #0] THEN CALL P"TC_EXIT" E1 A6043
    IF [#6044 EQ #0] THEN CALL P"TC_EXIT" E1 A6044
    IF [#6045 EQ #0] THEN CALL P"TC_EXIT" E1 A6045
    IF [#6046 EQ #0] THEN CALL P"TC_EXIT" E1 A6046
    IF [#6047 EQ #0] THEN CALL P"TC_EXIT" E1 A6047
    IF [#6048 EQ #0] THEN CALL P"TC_EXIT" E1 A6048
    IF [#6050 EQ #0] THEN CALL P"TC_EXIT" E1 A6050
    IF [#6051 EQ #0] THEN CALL P"TC_EXIT" E1 A6051
    IF [#6052 EQ #0] THEN CALL P"TC_EXIT" E1 A6052
    IF [#6053 EQ #0] THEN CALL P"TC_EXIT" E1 A6053
    IF [#6054 EQ #0] THEN CALL P"TC_EXIT" E1 A6054
    IF [#6055 EQ #0] THEN CALL P"TC_EXIT" E1 A6055
    IF [#6056 EQ #0] THEN CALL P"TC_EXIT" E1 A6056
    IF [#6057 EQ #0] THEN CALL P"TC_EXIT" E1 A6057
    IF [#6058 EQ #0] THEN CALL P"TC_EXIT" E1 A6058
    IF [#6059 EQ #0] THEN CALL P"TC_EXIT" E1 A6059
    IF [#6060 EQ #0] THEN CALL P"TC_EXIT" E1 A6060

    ; define tool change generic parameters
    #<tc.type>=#6000
    #<tc.features>=#6001
    #<tc.feed>=#6009
    #<tc.enable_tool_length_compensation>=#6016

    ; define manual tool change parameters
    #<tc.mtc.change_position.x>=#6002
    #<tc.mtc.change_position.y>=#6003
    #<tc.mtc.change_position.z>=#6004

    ; define tls (tool length presetter) parameters
    #<tc.tls.safe_position.z>=#6005
    #<tc.tls.top_center_position.x>=#6006
    #<tc.tls.top_center_position.y>=#6007
    #<tc.tls.top_center_position.z>=#6008
    #<tc.tls.approaching_position.z>=#6010
    #<tc.tls.approaching_feed>=#6011
    #<tc.tls.1st_capture_feed>=#6012
    #<tc.tls.2nd_capture_feed>=#6013
    #<tc.tls.up_movement_after_1st_touch>=#6014
    #<tc.tls.minimum_tool_lenght_accepted>=#6015
    #<tc.tls.probe.cleaning.output>=#6017
    #<tc.tls.probe.cleaning.output.time>=#6018

    ; define atc parameters
    #<tc.atc.magazine.type>=#6020
    #<tc.atc.magazine.input.open>=#6021
    #<tc.atc.magazine.input.closed>=#6022
    #<tc.atc.magazine.input.timeout>=#6023
    #<tc.atc.magazine.output.time>=#6024
    #<tc.atc.magazine.output.open>=#6025
    #<tc.atc.magazine.output.close>=#6026

    #<tc.atc.safe.z>=#6050
    #<tc.atc.unlock.input>=#6051
    #<tc.atc.unlock.output>=#6052
    #<tc.atc.unlock.timeout>=#6053
    #<tc.atc.clamp_movement>=#6054
    #<tc.atc.pick_distance.z>=#6055
    #<tc.atc.enable_tool_setter>=#6056
    #<tc.atc.spindle.s1>=#6057
    #<tc.atc.spindle.s2>=#6058
    #<tc.atc.slot.axis.orientation>=#6059

    #<tc.atc.fast.unload.tool.holder.distance>=#6036
    #<tc.atc.slow.unload.tool.holder.distance>=#6037
    #<tc.atc.slow.unload.tool.holder.feed>=#6038
    #<tc.atc.fast.load.tool.holder.distance>=#6039
    #<tc.atc.medium.load.tool.holder.distance>=#6040
    #<tc.atc.medium.load.tool.holder.feed>=#6041
    #<tc.atc.final.load.tool.holder.feed>=#6042
    #<tc.atc.cone.load.unload.time>=#6043
    #<tc.atc.cone.load.unload.input>=#6044
    #<tc.atc.cone.cleaning.blow.output>=#6045
    #<tc.atc.bearing.cleaning.blow.output>=#6046
    #<tc.atc.cone.grip.output>=#6047
    #<tc.atc.cone.grip.output.time>=#6048

    ; check slots number and get slots positions parameters address
    #<tc.atc.slots>=#6060
    IF [#<tc.atc.slots> LT 0] THEN CALL P"TC_EXIT" E8 A6060
    IF [[#6049 EQ #0] OR [#6049 EQ 0]] THEN10
        IF [#<tc.atc.slots> GT 13] THEN CALL P"TC_EXIT" E8 A6060
        #<tc.atc.slot.address>=6061
    ELSE10
        #<tc.atc.slot.address>=#6049
        IF [[#<tc.atc.slot.address> + [#<tc.atc.slots> * 3] GE 7000] THEN CALL P"TC_EXIT" E8 A6049
    END10

    ; recover atc slots positions (MCS)
    #<_i_>=1
    #<_j_>=#<tc.atc.slot.address>
    WHILE [#<_i_> LE #<tc.atc.slots>] DO10
        IF [##<_j_> EQ #0] THEN CALL P"TC_EXIT" E1 A#<_j_>
        #<tc.atc.slots[#<_i_>].x>=##<_j_>
        #<_j_>=[#<_j_> + 1]
        IF [##<_j_> EQ #0] THEN CALL P"TC_EXIT" E1 A#<_j_>
        #<tc.atc.slots[#<_i_>].y>=##<_j_>
        #<_j_>=[#<_j_> + 1]
        IF [##<_j_> EQ #0] THEN CALL P"TC_EXIT" E1 A#<_j_>
        #<tc.atc.slots[#<_i_>].z>=##<_j_>
        #<_j_>=[#<_j_> + 1]
        #<_i_>=[#<_i_> + 1]
    END10

ENDSUB