diff --git a/klippy/extras/a_axis_offset.py b/klippy/extras/a_axis_offset.py index 50c0acd36ab3..7098691556cb 100644 --- a/klippy/extras/a_axis_offset.py +++ b/klippy/extras/a_axis_offset.py @@ -1,6 +1,8 @@ # Helper script to automate a axis offset calculation import math +import logging + RAD_TO_DEG = 57.295779513 @@ -10,18 +12,24 @@ def __init__(self, config): self.printer = config.get_printer() self.gcode_move = self.printer.load_object(config, 'gcode_move') self.gcode = self.printer.lookup_object('gcode') + self.calc_offset = 0. + self.max_repeat_probe = config.getint('max_repeat_probe', 5) + self.threshold_value = config.getfloat('threshold_value', 0.02) self.point_coords = [ [0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0.] ] - self.gcode.register_command('CALC_A_AXIS_OFFSET', - self.cmd_CALC_A_AXIS_OFFSET, - desc=self.cmd_CALC_A_AXIS_OFFSET_help) + self.gcode.register_command( + 'ALIGN_A_AXIS', self.cmd_ALIGN_A_AXIS, + desc=self.cmd_ALIGN_A_AXIS_help) self.gcode.register_command( 'SAVE_A_AXIS_POINT', self.cmd_SAVE_A_AXIS_POINT, desc=self.cmd_SAVE_A_AXIS_POINT_help) + self.gcode.register_command( + 'MOVE_ALIGN_A_AXIS', self.cmd_MOVE_ALIGN_A_AXIS, + desc=self.cmd_MOVE_ALIGN_A_AXIS_help) cmd_SAVE_A_AXIS_POINT_help = "Save point for A axis offset" def cmd_SAVE_A_AXIS_POINT(self, gcmd): @@ -41,37 +49,46 @@ def cmd_SAVE_A_AXIS_POINT(self, gcmd): self.point_coords[point_idx][axis] = coord def _calc_a_axis_offset(self, point_0, point_1): - offset = RAD_TO_DEG * math.asin((point_1[2] - point_0[2]) / math.hypot( - point_1[1] - point_0[1], point_1[2] - point_0[2])) + """ + . b + . . + . . + a . . . . c + """ + bc = point_1[2] - point_0[2] + ac = point_1[1] - point_0[1] + ab = math.hypot(ac, bc) + asin_a = math.asin(bc / ab) + offset = RAD_TO_DEG * asin_a + logging.info("calculate offset for the axis A: %f." % offset) return offset - cmd_CALC_A_AXIS_OFFSET_help = "Calculate A axis offset" - def cmd_CALC_A_AXIS_OFFSET(self, gcmd): - offset = self._calc_a_axis_offset( - self.point_coords[0], self.point_coords[1]) + def _apply_offset_a(self): homing_origin_a = self.gcode_move.get_status()['homing_origin'].a - if homing_origin_a + offset > 0.0: - offset = 0.0 + if (homing_origin_a + self.calc_offset) > 0.0: + logging.warning("A-axis offset is positive") + self.calc_offset = 0.0 offset_gcmd = self.gcode.create_gcode_command( - 'SET_GCODE_OFFSET', 'SET_GCODE_OFFSET', {'A_ADJUST': offset}) + 'SET_GCODE_OFFSET', 'SET_GCODE_OFFSET', {'A_ADJUST': self.calc_offset}) self.gcode_move.cmd_SET_GCODE_OFFSET(offset_gcmd) - gcmd.respond_info("calculate offset for the axis A: %f." % offset) + logging.info("apply offset for the axis A: %f." % self.calc_offset) - # cmd_CALC_A_AXIS_OFFSET_TOOL_help = "Calculate A axis offset by tool" - # def cmd_CALC_A_AXIS_OFFSET_TOOL(self, gcmd): - # # get average value axis Z - # self.point_coords[0][2] = (self.point_coords[0][2] + self.point_coords[2][2]) / 2. - # self.point_coords[1][2] = (self.point_coords[1][2] + self.point_coords[3][2]) / 2. - # # calculate offset - # offset = self._calc_a_axis_offset( - # self.point_coords[0], self.point_coords[1]) - # homing_origin_a = self.gcode_move.get_status()['homing_origin'].a - # if homing_origin_a + offset > 0.0: - # offset = 0.0 - # offset_gcmd = self.gcode.create_gcode_command( - # 'SET_GCODE_OFFSET', 'SET_GCODE_OFFSET', {'A_ADJUST': offset}) - # self.gcode_move.cmd_SET_GCODE_OFFSET(offset_gcmd) - # gcmd.respond_info("calculate offset for the axis A: %f." % offset) + cmd_ALIGN_A_AXIS_help = "Calculate A axis offset" + def cmd_ALIGN_A_AXIS(self, gcmd): + mode = gcmd.get('MODE') + for i in range(self.max_repeat_probe): + self.gcode.run_script_from_command("MOVE_ALIGN_A_AXIS MODE=%s" % mode) + self.calc_offset = self._calc_a_axis_offset(self.point_coords[0], self.point_coords[1]) + if abs(self.calc_offset) <= self.threshold_value: + break + else: + self._apply_offset_a() + + cmd_MOVE_ALIGN_A_AXIS_help = "By default, it returns the offset of the axis,\ + if necessary, perform rename_existing of this command in the macro" + def cmd_MOVE_ALIGN_A_AXIS(self, gcmd): + homing_origin_a = self.gcode_move.get_status()['homing_origin'].a + gcmd.respond_info("offset A-axis= %s" % homing_origin_a) def load_config(config): diff --git a/stereotech_config/750/calibrate/probe_v3.cfg b/stereotech_config/750/calibrate/probe_v3.cfg index 79193c069e3a..bd2251afb930 100644 --- a/stereotech_config/750/calibrate/probe_v3.cfg +++ b/stereotech_config/750/calibrate/probe_v3.cfg @@ -56,7 +56,7 @@ gcode: ; set approximate radius, length and measuring the wcs_1_z CALC_TOOL_PARAMS LENGTH=1 APPROXIMATE_RADIUS=1 ; checking and apply offset for axis A - ALIGN_A_AXIS_TOOL + ALIGN_A_AXIS MODE=tool ; move for measuring the wcs_2_y PROBE_TOOL_POINT POINT=A_Y_A90 ADJUST_BASEMENT_WCS_V2 WCS=2 @@ -107,7 +107,7 @@ gcode: {% set tool_length = printer['gcode_macro CALC_TOOL_PARAMS'].length|float %} {% if radius < 5.0 and tool_length > 35.0 %} ; checking and apply offset for axis A - ALIGN_A_AXIS_TOOL + ALIGN_A_AXIS MODE=tool ; move for measuring the wcs_2_x PROBE_TOOL_POINT POINT=A_X_A90 SET_POINT MACRO=SAVE_WCS_CALC_POINT POINT=0 diff --git a/stereotech_config/calibrate/probe_5d.cfg b/stereotech_config/calibrate/probe_5d.cfg index 948363157f51..14f368ade293 100644 --- a/stereotech_config/calibrate/probe_5d.cfg +++ b/stereotech_config/calibrate/probe_5d.cfg @@ -197,3 +197,20 @@ gcode: {% set wcs_4 = printer.gcode_move.wcs_offsets[4] %} G10 L2 P2 X{wcs_3[0]} Y{wcs_3[1]} Z{wcs_3[2]} G10 L2 P3 X{wcs_4[0]} Y{wcs_4[1]} Z{wcs_4[2]} + +[gcode_macro MOVE_ALIGN_A_AXIS] +description: movements to obtain the points necessary to calculate the offset of the A axis +rename_existing: MOVE_ALIGN_A_AXIS_OLD +gcode: + {% set mode = params.MODE %} + {% if mode == 'template' %} + PROBE_TEMPLATE_POINT POINT=A_Z + SET_POINT MACRO=SAVE_A_AXIS_POINT POINT=0 + PROBE_TEMPLATE_POINT POINT=B_Z + SET_POINT MACRO=SAVE_A_AXIS_POINT POINT=1 + {% elif mode == 'tool' %} + PROBE_TOOL_POINT POINT=B_Z_A90 + SET_POINT MACRO=SAVE_A_AXIS_POINT POINT=0 + PROBE_TOOL_POINT POINT=A_Z_A90 + SET_POINT MACRO=SAVE_A_AXIS_POINT POINT=1 + {% endif %} diff --git a/stereotech_config/calibrate/probe_5d_template.cfg b/stereotech_config/calibrate/probe_5d_template.cfg index c8276986e1d8..e6179bfd9e44 100644 --- a/stereotech_config/calibrate/probe_5d_template.cfg +++ b/stereotech_config/calibrate/probe_5d_template.cfg @@ -8,7 +8,7 @@ gcode: G0 C0 ADJUST_TEMPLATE_HEIGHT A=0 ALIGN_C_AXIS - ALIGN_A_AXIS + ALIGN_A_AXIS MODE=template ADJUST_TEMPLATE_HEIGHT A=0 ADJUST_TEMPLATE_HEIGHT A=90 AUTO_WCS @@ -40,19 +40,6 @@ gcode: PROBE_TEMPLATE_POINT POINT=C_Y SET_POINT MACRO=SAVE_C_AXIS_POINT POINT=0 -[gcode_macro ALIGN_A_AXIS] -description: align template along horizontal plane -variable_repeat: 2 -gcode: - {% set repeat = printer["gcode_macro ALIGN_A_AXIS"].repeat %} - {% for idx in range(repeat) %} - PROBE_TEMPLATE_POINT POINT=A_Z - SET_POINT MACRO=SAVE_A_AXIS_POINT POINT=0 - PROBE_TEMPLATE_POINT POINT=B_Z - SET_POINT MACRO=SAVE_A_AXIS_POINT POINT=1 - CALC_A_AXIS_OFFSET - {% endfor %} - [gcode_macro MOVE_TO_AUTO_WCS] gcode: {% set set_xy = params.XY|default(0) %} diff --git a/stereotech_config/calibrate/probe_5d_tool.cfg b/stereotech_config/calibrate/probe_5d_tool.cfg index 1d907e428e49..cd7e70ca93d2 100644 --- a/stereotech_config/calibrate/probe_5d_tool.cfg +++ b/stereotech_config/calibrate/probe_5d_tool.cfg @@ -69,24 +69,6 @@ gcode: G10 L2 P{wcs + 1} {axis_names[axis]}{value} {% endif %} -[gcode_macro ALIGN_A_AXIS_TOOL] -description: align template along horizontal plane -variable_repeat: 2 -gcode: - {% set repeat = printer["gcode_macro ALIGN_A_AXIS_TOOL"].repeat %} - {% for idx in range(repeat) %} - PROBE_TOOL_POINT POINT=B_Z_A90 - SET_POINT MACRO=SAVE_A_AXIS_POINT POINT=0 - PROBE_TOOL_POINT POINT=A_Z_A90 - SET_POINT MACRO=SAVE_A_AXIS_POINT POINT=1 - # PROBE_TOOL_POINT POINT=B_Z_A90_C60 - # SET_POINT MACRO=SAVE_A_AXIS_POINT POINT=2 - # PROBE_TOOL_POINT POINT=A_Z_A90_C60 - # SET_POINT MACRO=SAVE_A_AXIS_POINT POINT=3 - # CALC_A_AXIS_OFFSET_TOOL - CALC_A_AXIS_OFFSET - {% endfor %} - [gcode_macro CHECK_SKEW_TOOL] description: This macro calculates the x-axis skew between four points using the average. gcode: diff --git a/stereotech_config/calibrate/probe_v2.cfg b/stereotech_config/calibrate/probe_v2.cfg index 7ade33cc2363..6a9d5ba4fc25 100644 --- a/stereotech_config/calibrate/probe_v2.cfg +++ b/stereotech_config/calibrate/probe_v2.cfg @@ -49,7 +49,7 @@ gcode: ; set approximate radius, length and measuring the wcs_1_z CALC_TOOL_PARAMS LENGTH=1 APPROXIMATE_RADIUS=1 ; checking and apply offset for axis A - ALIGN_A_AXIS_TOOL + ALIGN_A_AXIS MODE=tool ; move for measuring the wcs_2_y PROBE_TOOL_POINT POINT=A_Y_A90 ADJUST_BASEMENT_WCS_V2 WCS=2 @@ -100,7 +100,7 @@ gcode: {% set tool_length = printer['gcode_macro CALC_TOOL_PARAMS'].length|float %} {% if radius < 5.0 and tool_length > 35.0 %} ; checking and apply offset for axis A - ALIGN_A_AXIS_TOOL + ALIGN_A_AXIS MODE=tool ; move for measuring the wcs_2_x PROBE_TOOL_POINT POINT=A_X_A90 SET_POINT MACRO=SAVE_WCS_CALC_POINT POINT=0 diff --git a/test/klippy/five_axis.cfg b/test/klippy/five_axis.cfg index 9ed4ab97b3a1..148eddf01965 100644 --- a/test/klippy/five_axis.cfg +++ b/test/klippy/five_axis.cfg @@ -115,7 +115,7 @@ a: 0.0 c: 0.0 e: 0.0 -#[a_axis_offset] +[a_axis_offset] [wcs_1] x: 160.7 diff --git a/test/klippy/five_axis.test b/test/klippy/five_axis.test index 3e8a871e4a56..66aa88824b67 100644 --- a/test/klippy/five_axis.test +++ b/test/klippy/five_axis.test @@ -3,6 +3,6 @@ CONFIG five_axis.cfg G28 SAVE_A_AXIS_POINT POINT=0 COORDS=100.0,140.0,120.0 -SAVE_A_AXIS_POINT POINT=1 COORDS=100.0,90.0,110.0 -CALC_A_AXIS_OFFSET +SAVE_A_AXIS_POINT POINT=1 COORDS=100.0,90.0,119.5 +ALIGN_A_AXIS MODE=tool SAVE_CONFIG