toolchange.cfg

# Script to change back to the main extruder


#TODO: Save and load current tool across reboots:
# See https://www.klipper3d.org/Config_Reference.html#save_variables

#############################
# Aliases
#############################
[gcode_macro T0]
variable_color: "cd4c4c"
variable_extruder_name: "extruder"
variable_x_pos: 165.9
variable_y_pos: 2.85
variable_y_clear: 70
variable_z_clear: 8
variable_z_offset: 0
variable_extrude_on_load: 3 # bigger Z offset moves farther away. Increase value to move farther to positive.
gcode: 
    _T TOOL_ID=0
    
[gcode_macro T1]
variable_color: "6495ed"
variable_extruder_name: "extruder1"
variable_x_pos: 87.8
variable_y_pos: 2.85
variable_y_clear: 60 # How far the tool needs after unloading to freely move without a collision
variable_z_clear: 5 # how high z axis must be to safely change tools (useful for clearing bed clips)
variable_x_offset: 1.0 ; 5.75 # Positive value to more right (+x) negative to move left (-x)
variable_y_offset: -1.55 ; 1.05 # Positive value to move back (increase y), negative to move nozzle forward (lower y)
variable_z_offset: 2.05 # bigger Z offset moves farther away. Increase value to move farther to positive.
variable_extrude_on_load_len: 3 
variable_extrude_speed: 30
gcode: 
    _T TOOL_ID=1


############################
# Configuration
############################
[gcode_macro _initialize_toolchanger]
# TODO: Move the extruder variables to variable_'s under each extruder section.
# It might only be possible to set variables inside macros. 
# for more info, see: https://www.klipper3d.org/Config_Reference.html#delayed_gcode
gcode:
    # get save variables
    {% set svv = printer.save_variables.variables %}

    # Count all tools
    # Loop through a range and check if the tool macro number exists. 
    # Once an invalid tool macro is found, set the number of tools
    # and ignore all other values (jinja can't break a for loop)
    {% set tool_count = namespace(value=-1) %}
    {% for i in range(10) %} # if there are more than 10 tools, this will have to go up. MMU future?
        {% if tool_count.value == -1 %} # tool count hasn't already been set on a previous loop
            {% set tool_macro_name = "gcode_macro T" ~ (loop.index - 1) %} # T0, T1, T2, etc.
            {% set tool_macro = printer[tool_macro_name]|default(-1) %}

            # Test if the tool exists and tool_count hasn't already been set. 
            {% if tool_macro == -1 %}
                {% set tool_count.value = loop.index - 1 %}
                # RESPOND TYPE=echo MSG='Tool count: {tool_count.value}'
            # {% else %}
            #     RESPOND TYPE=echo MSG='Testing tool {loop.index - 1} exists with extruder: {tool_macro.extruder_name}'
            {% endif %}

        {% endif %} # endif tool count
    {% endfor %}
    SAVE_VARIABLE VARIABLE=tool_count VALUE={tool_count.value|default(-1)}
    #################################### End Testing 2 #####################################

    # Y clearance when moving between tools, with no tool loaded
    SAVE_VARIABLE VARIABLE=toolchanger_lock_clearance VALUE=20
    
    # how fast to move when loading/unloading a tool (in mm/minute)
    SAVE_VARIABLE VARIABLE=toolchanger_load_speed VALUE=3000
    SAVE_VARIABLE VARIABLE=toolchanger_travel_speed VALUE=10000
    SAVE_VARIABLE VARIABLE=toolchanger_z_speed VALUE=3000
    SAVE_VARIABLE VARIABLE=toolchanger_z_raise VALUE=0.5 ; default z raise while changing the tool
    # how long to wait for servo to lock/unlock (in milliseconds)
    SAVE_VARIABLE VARIABLE=toolchanger_servo_delay VALUE=400
    SAVE_VARIABLE VARIABLE=toolchanger_servo_lock_angle VALUE=180
    SAVE_VARIABLE VARIABLE=toolchanger_servo_unlock_angle VALUE=25

    # load configuration
    ACTIVATE_EXTRUDER_BY_ID TOOL_ID={svv.current_extruder_id|default(0)} APPLY_OFFSETS=1


# Load the configuration and variables
[delayed_gcode toolchanger_setup]
gcode: _initialize_toolchanger
initial_duration: 0.4


############################
# ACTIVATE_EXTRUDER_BY_ID {TOOL_ID}
# 
# Activates a tool and extruder by the tool ID
# 
#   {TOOL_ID} - ID of the tool to load (default 0)
#   {APPLY_OFFSETS} - 0 to not apply tool offsets, 1 to apply (default 1)
#   {MOVE} - 1 to immediately move tool to new offsets. 0 do not move tool to offsets. (default 0)
############################
[gcode_macro ACTIVATE_EXTRUDER_BY_ID]
gcode: 
    # get TOOL_ID param
    {% set tool_id = params.TOOL_ID|default(0)|int %} # default to first tool

    # get save variables
    {% set svv = printer.save_variables.variables %}
    # ensure tool_id is less than the total number of tools
    {% if tool_id < svv.tool_count %} #tool is 0 indexed, count is just 1, 2, 3...
        # get the tool object
        {% set tool_macro_name = "gcode_macro T" ~ tool_id %}
        {% set tool_macro = printer[tool_macro_name] %}

        # activate extruder
        ACTIVATE_EXTRUDER extruder={tool_macro.extruder_name}
        # remember that this extruder was set
        SAVE_VARIABLE VARIABLE=current_extruder_id VALUE={tool_id}

        # Optional: Apply offsets
        {% if params.APPLY_OFFSETS|default(1)|int == 1 %}
            # set move speed gcode to nothing if no speed is set, otherwise MOVE_SPEED={speed}
            {% set move_speed_gcode = 'MOVE_SPEED=' ~ params.MOVE_SPEED|default(30)|float if params.MOVE_SPEED|default(0) else '' %}

            # apply tool offsets
            SET_GCODE_OFFSET {('X_ADJUST=' ~ tool_macro.x_offset) if 'x_offset' in tool_macro} {('Y_ADJUST=' ~ tool_macro.y_offset) if 'y_offset' in tool_macro}  {('Z_ADJUST=' ~ tool_macro.z_offset) if 'z_offset' in tool_macro} MOVE={params.MOVE|default(0)} {move_speed_gcode} #MOVE_SPEED=30
            # DEBUG
            # RESPOND TYPE=echo MSG='GCODE OFFSET ADJUST X:{('X_ADJUST=' ~ tool_macro.x_offset) if 'x_offset' in tool_macro} Y:{('Y_ADJUST=' ~ tool_macro.y_offset) if 'y_offset' in tool_macro} Z:{('Z_ADJUST=' ~ tool_macro.z_offset) if 'z_offset' in tool_macro} (ACTIVATE_EXTRUDER_BY_ID)'

        {% endif %} # apply offsets
        
    {% else %}
        RESPOND TYPE=error MSG='Invalid tool: T{tool_id}'
    {% endif %} # validate tool_id


[gcode_macro _T_UNLOAD_ANY] # Unload Tool
gcode:
    # get saved variables
    {% set svv = printer.save_variables.variables %}

    # unload the currently loaded tool
    _T_UNLOAD TOOL_ID={svv.current_extruder_id}


############################
# _T {TOOL_ID} {RETURN}=0 {RETURN_Z}=0
# 
#   {TOOL_ID} - ID of the tool to load
#   {RETURN} 0/1 - Optional. Whether to return to the start position, 
#       before toolchange started. Default 1 (return)
#   {RETURN_Z} 0/1 - Optional. Whether to return z to start position, 
#       before toolchange started. Overridden by RETURN.
#       Default 0 (do not return z) # TODO: Remove RETURN_Z or clarify that it will be overridden by Z_MOVE
#   {Z_MOVE} 0/1 - Automatically adjust z to avoid crashing tool. 
#       Might need to be disabled to allow z homing. (default 1) TODO: Add third option that forces z raise only?
############################
[gcode_macro _T]
gcode:
    # get printer save variables and save as 'svv'
    {% set svv = printer.save_variables.variables %}  
    # set an alias for the printer's coordinate origin (which holds the x, y, and z offsets)
    {% set origin = printer.gcode_move.homing_origin %}

    # get tool ID
    {% set new_tool_idx = params.TOOL_ID|default(0)|int %} # default to first tool

    # if the tool isn't loaded yet...
    {% if new_tool_idx|string() != svv.current_extruder_id|default(0)|string() %}

        # ensure new_tool_idx is less than the total number of tools
        {% if new_tool_idx < svv.tool_count %}
        
            # get the tool object
            {% set tool_macro_name = "gcode_macro T" ~ new_tool_idx %}
            {% set new_tool_macro = printer[tool_macro_name] %}

            #################
            # Save return position
            # Set x return position
            {% set x_offset = new_tool_macro.x_offset if 'x_offset' in new_tool_macro else 0 %}

            {% if (printer.toolhead.position.x|float + x_offset|float >= printer.toolhead.axis_maximum.x|float) %} # new position would exceed max
                {% set return_x = printer.toolhead.axis_maximum.x|float - x_offset|float - 1 %}
            {% elif (printer.toolhead.position.x|float + x_offset|float <= printer.toolhead.axis_minimum.x|float) %} # new position would exceed min
                {% set return_x = printer.toolhead.axis_minimum.x|float - x_offset|float + 1 %}
            {% else %} # new position fits in workspace
                {% set return_x = printer.toolhead.position.x|float - origin.x|float %}
            {% endif %}
            # Set y return position
            {% set y_offset = new_tool_macro.y_offset if 'y_offset' in new_tool_macro else 0 %}

            {% if (printer.toolhead.position.y|float + y_offset|float >= printer.toolhead.axis_maximum.y|float) %} # new position would exceed max
                {% set return_y = printer.toolhead.axis_maximum.y|float - y_offset|float - 1 %}
            {% elif (printer.toolhead.position.y|float + y_offset|float <= printer.toolhead.axis_minimum.y|float) %} # new position would exceed min
                {% set return_y = printer.toolhead.axis_minimum.y|float - y_offset|float %}
            {% else %} # new position fits in workspace
                {% set return_y = printer.toolhead.position.y|float - origin.y|float %}
            {% endif %}
            # Set z return position
            {% set z_offset = new_tool_macro.z_offset if 'z_offset' in new_tool_macro else 0 %}

            {% if (printer.toolhead.position.z|float + z_offset|float >= printer.toolhead.axis_maximum.z|float) %} # new position would exceed max
                {% set return_z = printer.toolhead.axis_maximum.z|float - z_offset|float - 1 %}
            {% elif (printer.toolhead.position.z|float + z_offset|float <= printer.toolhead.axis_minimum.z|float) %} # new position would exceed min
                {% set return_z = printer.toolhead.axis_minimum.z|float - z_offset|float %}
            {% else %} # new position fits in workspace
                {% set return_z = printer.toolhead.position.z|float - origin.z|float %}
            {% endif %}

            #DEBUG
            # RESPOND TYPE=echo MSG='Saved position X{return_x} Y{return_y}'

            ###################
            # Prep Z (raise a little)
            {% if params.Z_MOVE|default(1) %}
                SAVE_GCODE_STATE NAME=ORIGINAL_PRINTER_STATE
                M220 S100 ; reset speed to 0
                G91 ; relative movement
                ; raise z at least tool's z_clear mm (if defined), otherwise raise by toolchanger_z_raise
                {% if (('z_clear' in new_tool_macro) and (new_tool_macro.z_clear > return_z + svv.toolchanger_z_raise)) %} 
                    G1 Z{new_tool_macro.z_clear - return_z} F{svv.toolchanger_z_speed}
                {% else %}
                    G1 Z{svv.toolchanger_z_raise} F{svv.toolchanger_z_speed}
                {% endif %}
                G90 ; absolute movement
                RESTORE_GCODE_STATE NAME=ORIGINAL_PRINTER_STATE
            {% endif %}

            ###################
            # drop off old tool
            _T_UNLOAD_ANY Z_MOVE={params.Z_MOVE|default(1)}


            ###################
            # Save state/offsets/speed and any other settings
            SAVE_GCODE_STATE NAME=ORIGINAL_PRINTER_STATE
            # set x and y offsets to 0 to prevent crashing while changing tool_macro.
            SET_GCODE_OFFSET X=0 Y=0

            ###################
            # pick up new tool
            RESPOND TYPE=echo MSG='Loading tool {new_tool_idx}'

            M220 S100 ; reset speed to 0
            G90 ; absolute movement
            # move to x load position, y prep position
            G1 X{new_tool_macro.x_pos} Y{svv.toolchanger_lock_clearance} F{svv.toolchanger_travel_speed}
            # move in y
            G1 Y{new_tool_macro.y_pos} F{svv.toolchanger_load_speed}

            # lock servo, wait, disable servo
            SET_SERVO SERVO=tool ANGLE={svv.toolchanger_servo_lock_angle|int} # release tool
            G4 P{svv.toolchanger_servo_delay} # wait for servo to get into position
            SET_SERVO SERVO=tool WIDTH=0 #turn servo off

            # return speed etc to original values
            RESTORE_GCODE_STATE NAME=ORIGINAL_PRINTER_STATE


            # set z offset and move z to position
            SET_GCODE_OFFSET {('Z_ADJUST=' ~ new_tool_macro.z_offset) if 'z_offset' in new_tool_macro} MOVE={params.Z_MOVE|default(1)} MOVE_SPEED=30 # + printer.stepper_z.position_endstop|float}


            # Save speed to load variable
            SAVE_GCODE_STATE NAME=LOAD_SPEED_STATE
            M220 S100 ; reset speed to 0
            G90 ; absolute movement
            ACTIVATE_EXTRUDER_BY_ID TOOL_ID={new_tool_idx}

            # purge the currently loaded extruder, if extrude length set
            # _PURGE_EXTRUDER
            M83 ; relative extrusion mode
            G1 {('E' ~ new_tool_macro.extrude_on_load_len) if 'extrude_on_load_len' in new_tool_macro} {'F' ~ (new_tool_macro.extrude_speed*60) if 'extrude_speed' in new_tool_macro else (5*60)}

            {('Z_ADJUST=' ~ new_tool_macro.z_offset) if 'z_offset' in new_tool_macro}
            ###############################
            # Back out Y
            G1 Y{new_tool_macro.y_clear} F{svv.toolchanger_load_speed}
            # return speed etc to original values
            RESTORE_GCODE_STATE NAME=LOAD_SPEED_STATE


            # Apply final x/y offsets. Default to 0.
            SET_GCODE_OFFSET {'X=' ~ x_offset if 'x_offset' in new_tool_macro} {'Y=' ~ y_offset if 'y_offset' in new_tool_macro}


            ##############################
            # return to previous position
            SAVE_GCODE_STATE NAME=NEW_SPEED_STATE
            M220 S100 ; reset speed to 0
            G90 ; absolute movement

            RESPOND TYPE=echo MSG='Returning to position X{return_x} Y{return_y}'

            {% if params.RETURN|default(1) != 0 %}
                # return to start position
                G1 X{return_x} Y{return_y} F{svv.toolchanger_travel_speed} {('Z' ~ return_z) if params.Z_MOVE|default(1)} #Z{return_z}
            {% elif params.RETURN_Z|default(0) != 0 and params.Z_MOVE|default(1) %}
                # return Z to start height
                G1 Z{return_z} F{svv.toolchanger_travel_speed}
            {% endif %}

            RESTORE_GCODE_STATE NAME=NEW_SPEED_STATE

        {% else %}
            RESPOND TYPE=error MSG='Invalid tool: T{tool_id}'
        {% endif %}

    {% else %}
        RESPOND TYPE=error MSG='Tool {new_tool_idx} already loaded'
    {% endif %}


##############################
# _T_UNLOAD {TOOL_ID} {Z_MOVE}
# 
#   {Z_MOVE} 0/1 - Allow automatically adjustting z position.
#       Might need to be disabled to allow z homing. (default 1)
############################
[gcode_macro _T_UNLOAD] 
gcode:
    # alias for printer save variables
    {% set svv = printer.save_variables.variables %}    

    # get selected tool ID
    {% set tool_id = params.TOOL_ID|default(0)|int %} # default to first tool

    # ensure new_tool_idx is less than the total number of tools
    {% if tool_id >= svv.tool_count %}
        RESPOND TYPE=error MSG='Invalid tool: T{tool_id}'

    # check that the tool being unloaded is actually loaded
    {% elif svv.current_extruder_id|int == tool_id %}
        # get the tool object
        {% set tool_macro_name = "gcode_macro T" ~ tool_id %}
        {% set tool_macro = printer[tool_macro_name] %}

        RESPOND TYPE=echo MSG='Unloading tool {tool_id}'

        SAVE_GCODE_STATE NAME=UNLOAD_STATE_T
        M220 S100 ; reset speed to 0
        G90 ; absolute movement

        # temporarily unapply any x/y offsets to prevent crashing while changing tool
        SET_GCODE_OFFSET X=0 Y=0

        ###################
        # unload tool

        # move to x unload position, y prep position
        G1 X{tool_macro.x_pos} Y{tool_macro.y_clear} F{svv.toolchanger_travel_speed}

        # put tool in its mount
        G1 Y{tool_macro.y_pos} F{svv.toolchanger_load_speed}

        # unlock servo, wait, turn off servo
        SET_SERVO SERVO=tool ANGLE={svv.toolchanger_servo_unlock_angle|int} # release tool
        G4 P{svv.toolchanger_servo_delay} # wait for servo to get into position
        SET_SERVO SERVO=tool WIDTH=0 #turn servo off

        # back out y from the lock
        G1 Y{svv.toolchanger_lock_clearance} F{svv.toolchanger_load_speed}


        RESTORE_GCODE_STATE NAME=UNLOAD_STATE_T

        ## Unapply Offsets
        # Unapply x/y  offsets # TODO: Reenable unapplying offsets after tool is unloaded instead of setting x/y offsets to 0
        # SET_GCODE_OFFSET X_ADJUST=-{tool_macro.x_offset} Y_ADJUST=-{tool_macro.y_offset} MOVE=0 # CHANGED from setting offsets to 0
        SET_GCODE_OFFSET X=0 Y=0

        # Unapply Z offset
        {% if params.Z_MOVE|default(1) %}
            SET_GCODE_OFFSET {('Z_ADJUST=-' ~ tool_macro.z_offset) if 'z_offset' in tool_macro} MOVE=1 MOVE_SPEED=40
        {% endif %}
        # DEBUG
        # RESPOND TYPE=echo MSG='gcode offset {('Z_ADJUST=-' ~ tool_macro.z_offset) if 'z_offset' in tool_macro}: _T_UNLOAD'


    {% else %}
        RESPOND TYPE=error MSG='Unable to unload Tool {tool_id}: not loaded'
    {% endif %}



#########################
# Offsets
# 
# Tape a pin to the bed and position 
# the main extruder directly over it.
# Note the position with M114 and put it
# in the xy_calibrate_position variables.
# Next, load the new extruder and run
# this macro. Adjust the position as
# necessary then run the 'get_new_offset'
# macro to see the new offsets. 
# Put the new offsets in the tool
# configuration.

# switch to tool 0
# position toolhead directly over the pin
# run xy_calibrate_position
# raise 10mm
# switch tools
# position toolhead directly over the pin
# lower appropriate amount (may be less than 10mm!)
# run set_new_offset
# set new offsets in the configuration
# set exact z offset for new nozzle
#########################
[gcode_macro xy_calibrate_position]
variable_x_pos: 19.1
variable_y_pos: 78.5
variable_z_pos: 12.2
# variable_speed: 100 # in mm/s
# variable_z_speed: 30 # in mm/s
gcode:
    SAVE_GCODE_STATE NAME=CALIBRATE_POS_STATE


    # store current position
    SET_GCODE_OFFSET X=0 Y=0 Z=0 # TODO: Does probe or stepper_z position_endstop need to applied while setting z offset?s
    SET_GCODE_VARIABLE MACRO=xy_calibrate_position VARIABLE=x_pos VALUE={printer.toolhead.position.x|float}
    SET_GCODE_VARIABLE MACRO=xy_calibrate_position VARIABLE=y_pos VALUE={printer.toolhead.position.y|float}
    SET_GCODE_VARIABLE MACRO=xy_calibrate_position VARIABLE=z_pos VALUE={printer.toolhead.position.z|float}

    RESTORE_GCODE_STATE NAME=CALIBRATE_POS_STATE

[gcode_macro set_new_offset]
##########################
# Run this macro once the 
# tool offset seems right. 
gcode:
    {% set svv = printer.save_variables.variables %}

    SET_GCODE_OFFSET X=0 Y=0 Z=0

    # calculate new offset
    {% set x_offset = printer["gcode_macro xy_calibrate_position"].x_pos|float - printer.toolhead.position.x|float %}
    {% set y_offset = printer["gcode_macro xy_calibrate_position"].y_pos|float - printer.toolhead.position.y|float %}
    {% set z_offset = printer["gcode_macro xy_calibrate_position"].z_pos|float - printer.toolhead.position.z|float %}

    RESPOND TYPE=echo MSG='Set the following offsets for Tool {svv.current_extruder_id}: X:{x_offset}, Y:{y_offset}, Z(approximate):{z_offset}'

    # apply new offset
    SET_GCODE_OFFSET X={x_offset} Y={y_offset}