ATC Tool Rack implementation gesucht

%
(MSG, --- Start of toolchange.ngc ---)

o<toolchange> sub

    (MSG, --- Toolchange Subroutine Entry ---)

    ; ------------------------------------------------------------------
    ; A) Named parameters (populated by stdglue.py / M6 prolog):
    ;    #<selected_tool>, #<tool_in_spindle>,
    ;    #<selected_pocket>, #<current_pocket>,
    ;    #<atc_z_tool_change_height>, #<atc_z_tool_clearance_height>, etc.
    ; ------------------------------------------------------------------

    (MSG, Selected tool = #<selected_tool>)
    (MSG, Tool in spindle = #<tool_in_spindle>)
    (MSG, Selected pocket = #<selected_pocket>)
    (MSG, Current pocket = #<current_pocket>)

    ; ------------------------------------------------------------------
    ; B) Check if a tool change is actually needed
    ; ------------------------------------------------------------------
    IF [#<selected_tool> EQ #<tool_in_spindle>] THEN
        (MSG, Tool #<tool_in_spindle> is already loaded. No change needed.)
        M99
    END-IF

    ; ------------------------------------------------------------------
    ; C) Stop the spindle
    ; ------------------------------------------------------------------
    M5  ; no spindle orientation (M19) requested

    ; ------------------------------------------------------------------
    ; D) Rack coordinates (adjust as needed for your machine)
    ; ------------------------------------------------------------------
    #<rack_x>      = 1123
    #<rack_y_base> = 164
    #<rack_y_pitch> = 50

    ; We comment these out so they are NOT overwritten (assuming INI defines them):
    ; #<atc_z_tool_change_height>    = -80
    ; #<atc_z_tool_clearance_height> = 0

    ; Move Z to clearance
    G53 G0 Z[#<atc_z_tool_clearance_height>]

    ; ------------------------------------------------------------------
    ; E) Drop off the old tool (#<tool_in_spindle>) in #<current_pocket>
    ; ------------------------------------------------------------------
    IF [#<tool_in_spindle> GT 0] THEN

        ; 1) Y coordinate for the current pocket
        #<old_pocket_y> = [#<rack_y_base> + ([#<current_pocket>] - 1) * #<rack_y_pitch>]

        ; 2) Move to that pocket in machine coords (G53)
        G53 G0 X[#<rack_x>] Y[#<old_pocket_y>]

        ; 3) Lower Z to the tool-change height
        G53 G0 Z[#<atc_z_tool_change_height>]

        ; 4) Unclamp (M64 P2) to release old tool
        (MSG, Unclamping old tool with M64 P2)
        M64 P2
        G4 P1  ; dwell 1 second, adjust as needed

        ; 5) Move back up to clearance
        G53 G0 Z[#<atc_z_tool_clearance_height>]

        ; 6) Update the pocket table (#4001..#4024)
        #<pocket_index> = [4000 + #<current_pocket>]
        #[#<pocket_index>] = #<tool_in_spindle>
    END-IF

    ; ------------------------------------------------------------------
    ; F) Pick up the new tool (#<selected_tool>) from #<selected_pocket>
    ; ------------------------------------------------------------------
    IF [#<selected_tool> GT 0] THEN

        ; 1) Optional check: does pocket #<selected_pocket> hold #<selected_tool>?
        #<pocket_index> = [4000 + #<selected_pocket>]
        IF [#[#<pocket_index>] NE #<selected_tool>] THEN
            (MSG, *** WARNING: Pocket #<selected_pocket> does not match tool #<selected_tool>!)
        END-IF

        ; 2) Compute Y coordinate for the selected pocket
        #<new_pocket_y> = [#<rack_y_base> + ([#<selected_pocket>] - 1) * #<rack_y_pitch>]

        ; 3) Move to that pocket
        G53 G0 X[#<rack_x>] Y[#<new_pocket_y>]

        ; 4) Lower Z to the tool-change height
        G53 G0 Z[#<atc_z_tool_change_height>]

        ; 5) Clamp (M65 P2) to pick up the new tool
        (MSG, Clamping new tool with M65 P2)
        M65 P2
        G4 P1  ; dwell 1 second, adjust as needed

        ; 6) Return to clearance
        G53 G0 Z[#<atc_z_tool_clearance_height>]

        ; 7) This pocket is now empty
        #[#<pocket_index>] = 0
    END-IF

    ; ------------------------------------------------------------------
    ; G) The spindle now has the new tool
    ; ------------------------------------------------------------------
    #<tool_in_spindle> = #<selected_tool>

    ; ------------------------------------------------------------------
    ; H) Tell LinuxCNC the new tool is active & load offset
    ; ------------------------------------------------------------------
    M61 Q[#<selected_tool>]     ; set active tool to #<selected_tool>
    G43 H[#<selected_tool>]     ; load that tool's offset from table

    ; ------------------------------------------------------------------
    ; I) Return to caller
    ; ------------------------------------------------------------------
    M99

o<toolchange> endsub

M2

(MSG, --- End of toolchange.ngc ---)
%