################################################
###### VIBRATIONS AND SPEED OPTIMIZATIONS ######
################################################
# Written by Frix_x#0161 #
# @version: 2.1

# CHANGELOG:
#   v2.1: allow decimal entries for speed and increment and added the E axis as an option to be neasured
#   v2.0: added the possibility to measure mutliple axis
#   v1.0: first speed and vibrations optimization macro


### What is it ? ###
# This macro helps you to identify the speed settings that exacerbate the vibrations of the machine (ie. where the frame resonate badly).
# It also helps to find the clean speed ranges where the machine is silent.
# I had some strong vibrations at very specific speeds on my machine (52mm/s for example) and I wanted to find all these problematic speeds
# to avoid them in my slicer profile and finally get the silent machine I was dreaming!

# It works by moving the toolhead at different speed settings while recording the vibrations using the ADXL chip. Then the macro call a custom script
# to compute and find the best speed settings. The results can be found in your config folder using Fluidd/Mainsail file manager.

# The goal is to make it easy to set, share and use it.

# This macro is parametric and most of the values can be adjusted with their respective input parameters.
# It can be called without any parameters - in which case the default values would be used - or with any combination of parameters as desired.

# Usage:
#   1. DO YOUR INPUT SHAPER CALIBRATION FIRST !!! This macro should not be used before as it would be useless and the results invalid.
#   2. Call the VIBRATIONS_CALIBRATION macro with the speed range you want to measure (default 20 to 200mm/s with 2mm/s increment).
#      Be carefull about the Z_HEIGHT variable that default to 20mm -> if your ADXL is under the nozzle, increase it to avoid a crash of the ADXL on the bed of the machine.
#   3. Wait for it to finish all the measurement and compute the graph. Then look at it in the results folder.


[gcode_macro VIBRATIONS_CALIBRATION]
gcode:
    #
    # PARAMETERS
    #
    {% set size = params.SIZE|default(60)|int %} # size of the area where the movements are done
    {% set direction = params.DIRECTION|default('XY') %} # can be set to either XY, AB, ABXY, A, B, X, Y, Z
    {% set z_height = params.Z_HEIGHT|default(20)|int %} # z height to put the toolhead before starting the movements
    {% set verbose = params.VERBOSE|default(true) %} # Wether to log the current speed in the console

    {% set min_speed = params.MIN_SPEED|default(20)|float * 60 %} # minimum feedrate for the movements
    {% set max_speed = params.MAX_SPEED|default(200)|float * 60 %} # maximum feedrate for the movements
    {% set speed_increment = params.SPEED_INCREMENT|default(2)|float * 60 %} # feedrate increment between each move
    {% set feedrate_travel = params.TRAVEL_SPEED|default(200)|int * 60 %} # travel feedrate between moves
    {% set accel = params.ACCEL|default(config.printer["max_accel"])|float %}
    {% set accel_chip = params.ACCEL_CHIP|default("adxl345") %} # ADXL chip name in the config

    #
    # COMPUTED VALUES
    #
    {% set mid_x = printer.toolhead.axis_maximum.x|float / 2 %}
    {% set mid_y = printer.toolhead.axis_maximum.y|float / 2 %}
    {% set nb_samples = ((max_speed - min_speed) / speed_increment + 1) | int %}

    {% set direction_factor = {
        'XY'  : {
            'start' : {'x': -0.5, 'y': -0.5 },
            'move_factors' : {
                '0' : {'x': 0.5, 'y': -0.5, 'z': 0.0 },
                '1' : {'x': 0.5, 'y': 0.5, 'z': 0.0 },
                '2' : {'x': -0.5, 'y': 0.5, 'z': 0.0 },
                '3' : {'x': -0.5, 'y': -0.5, 'z': 0.0 }
                }
            },
        'AB' : {
            'start' : {'x': 0.0, 'y': 0.0 },
            'move_factors' : {
                '0' : {'x': 0.5, 'y': -0.5, 'z': 0.0 },
                '1' : {'x': -0.5, 'y': 0.5, 'z': 0.0 },
                '2' : {'x': 0.0, 'y': 0.0, 'z': 0.0 },
                '3' : {'x': 0.5, 'y': 0.5, 'z': 0.0 },
                '4' : {'x': -0.5, 'y': -0.5, 'z': 0.0 },
                '5' : {'x': 0.0, 'y': 0.0, 'z': 0.0 }
                }
            },
        'ABXY' : {
            'start' : {'x': -0.5, 'y': 0.5 },
            'move_factors' : {
                '0' : {'x': -0.5, 'y': -0.5, 'z': 0.0 },
                '1' : {'x': 0.5, 'y': -0.5, 'z': 0.0 },
                '2' : {'x': -0.5, 'y': 0.5, 'z': 0.0 },
                '3' : {'x': 0.5, 'y': 0.5, 'z': 0.0 },
                '4' : {'x': -0.5, 'y': -0.5, 'z': 0.0 },
                '5' : {'x': -0.5, 'y': 0.5, 'z': 0.0 }
                }
            },
        'B'  : {
            'start' : {'x': 0.5, 'y': 0.5 },
            'move_factors' : {
                '0' : {'x': -0.5, 'y': -0.5, 'z': 0.0 },
                '1' : {'x': 0.5, 'y': 0.5, 'z': 0.0 }
                }
            },
        'A'  : {
            'start' : {'x': -0.5, 'y': 0.5 },
            'move_factors' : {
                '0' : {'x': 0.5, 'y': -0.5, 'z': 0.0 },
                '1' : {'x': -0.5, 'y': 0.5, 'z': 0.0 }
                }
            },
        'X'  : {
            'start' : {'x': -0.5, 'y': 0.0 },
            'move_factors' : {
                '0' : {'x': 0.5, 'y': 0.0, 'z': 0.0 },
                '1' : {'x': -0.5, 'y': 0.0, 'z': 0.0 }
                }
            },
        'Y'  : {
            'start' : {'x': 0.0, 'y': 0.5 },
            'move_factors' : {
                '0' : {'x': 0.0, 'y': -0.5, 'z': 0.0 },
                '1' : {'x': 0.0, 'y': 0.5, 'z': 0.0 }
                }
            },
        'Z'  : {
            'start' : {'x': 0.0, 'y': 0.0 },
            'move_factors' : {
                '0' : {'x': 0.0, 'y': 0.0, 'z': 1.0 },
                '1' : {'x': 0.0, 'y': 0.0, 'z': 0.0 }
                }
            },
        'E'  : {
            'start' : {'x': 0.0, 'y': 0.0 },
            'move_factor' : 0.05
            }
        }
    %}

    #
    # STARTING...
    #
    {% if not 'xyz' in printer.toolhead.homed_axes %}
        { action_raise_error("Must Home printer first!") }
    {% endif %}

    {% if params.SPEED_INCREMENT|default(2)|float * 100 != (params.SPEED_INCREMENT|default(2)|float * 100)|int %}
        { action_raise_error("Only 2 decimal digits are allowed for SPEED_INCREMENT") }
    {% endif %}

    {% if (size / (max_speed / 60)) < 0.25 and direction != 'E' %}
        { action_raise_error("SIZE is too small for this MAX_SPEED. Increase SIZE or decrease MAX_SPEED!") }
    {% endif %}

    {% if not (direction in direction_factor) %}
        { action_raise_error("DIRECTION is not valid. Only XY, AB, ABXY, A, B, X, Y, Z or E is allowed!") }
    {% endif %}

    {action_respond_info("")}
    {action_respond_info("Starting speed and vibration calibration")}
    {action_respond_info("This operation can not be interrupted by normal means. Hit the \"emergency stop\" button to stop it if needed")}
    {action_respond_info("")}

    SAVE_GCODE_STATE NAME=STATE_VIBRATIONS_CALIBRATION

    M83
    G90
    SET_ACCEL_LIMIT ACCEL={accel} ACCEL_TO_DECEL={accel}
    # Going to the start position
    G1 Z{z_height}
    G1 X{mid_x + (size * direction_factor[direction].start.x) } Y{mid_y + (size * direction_factor[direction].start.y)} F{feedrate_travel}

    # vibration pattern for each frequency
    {% for curr_sample in range(0, nb_samples) %}
        {% set curr_speed = min_speed + curr_sample * speed_increment %}
        {% if verbose %}
            RESPOND MSG="{"Current speed: %.2f mm/s" % (curr_speed / 60)|float}"
        {% endif %}

        ACCELEROMETER_MEASURE CHIP={accel_chip}
        {% if direction == 'E' %}
            G0 E{curr_speed*direction_factor[direction].move_factor} F{curr_speed}
        {% else %}
            {% for key, factor in direction_factor[direction].move_factors|dictsort %}
                G1 X{mid_x + (size * factor.x) } Y{mid_y + (size * factor.y)} Z{z_height + (size * factor.z)} F{curr_speed}
            {% endfor %}
        {% endif %}
        ACCELEROMETER_MEASURE CHIP={accel_chip} NAME=sp{("%.2f" % (curr_speed / 60)|float)|replace('.','_')}n1

        G4 P300
        M400
    {% endfor %}

    {% if verbose %}
        RESPOND MSG="Graphs generation... Please wait a minute or two and look in the configured folder."
    {% endif %}
    RUN_SHELL_COMMAND CMD=plot_graph PARAMS="VIBRATIONS {direction}"

    RESTORE_GCODE_STATE NAME=STATE_VIBRATIONS_CALIBRATION