diff --git a/klippy/extras/auto_wcs.py b/klippy/extras/auto_wcs.py index c7f6ddb443f6..e54e352201a9 100644 --- a/klippy/extras/auto_wcs.py +++ b/klippy/extras/auto_wcs.py @@ -25,6 +25,12 @@ def __init__(self, config): [0., 0., 0.], [0., 0., 0.] ] + self.probe_backlash_x = 0. + self.probe_backlash_y = 0. + self.probe_backlash_y_2 = 0. + self.tooling_radius = 0. + self.tooling_radius_1 = 0. + self.tooling_radius_2 = 0. self.adjust_angle = 10 / RAD_TO_DEG self.gcode = self.printer.lookup_object('gcode') self.gcode.register_command( @@ -84,6 +90,74 @@ def _calc_wcs_2_new_sensor(self, thickness, adj, gcmd): z = self.point_coords[4][2] - (len_thickness - adj) return x, y, z + def calculate_probe_backlash(self, x1, y1, y2): + self.probe_backlash_x = abs(self.point_coords[3][0] - (x1 + 55)) + self.probe_backlash_y = abs(self.point_coords[5][1] - y2) + self.probe_backlash_y_2 = abs(self.point_coords[1][1] - (y1 - 5)) + + def cmd_GET_RADIUS_TOOLING(self, gcmd): + x1, y1 = self.point_coords[1][0] + self.probe_backlash_y, self.point_coords[1][1] + x2, y2 = self.point_coords[0][0], self.point_coords[0][1] + self.probe_backlash_y + x3, y3 = self.point_coords[2][0] - self.probe_backlash_y, self.point_coords[2][1] + c = (x1-x2)**2 + (y1-y2)**2 + a = (x2-x3)**2 + (y2-y3)**2 + b = (x3-x1)**2 + (y3-y1)**2 + s = 2*(a*b + b*c + c*a) - (a*a + b*b + c*c) + px = (a*(b+c-a)*x1 + b*(c+a-b)*x2 + c*(a+b-c)*x3) / s + py = (a*(b+c-a)*y1 + b*(c+a-b)*y2 + c*(a+b-c)*y3) / s + ar = a**0.5 + br = b**0.5 + cr = c**0.5 + r = ar*br*cr / ((ar+br+cr)*(-ar+br+cr)*(ar-br+cr)*(ar+br-cr))**0.5 + self.tooling_radius = r + gcmd.respond_info('radius_tooling= %s,(only backlash_y) centr_tool(%s;%s)' % ( + self.tooling_radius, px, py)) + self.get_radius_1(gcmd) + self.get_radius_2(gcmd) + return px, py, r + cmd_GET_RADIUS_TOOLING_help = "command for get the tooling radius from measuring points." + + def get_radius_1(self, gcmd): + # calculate radius whis probe_backlash_y + x1, y1 = self.point_coords[1][0] + self.probe_backlash_x, self.point_coords[1][1] + x2, y2 = self.point_coords[0][0], self.point_coords[0][1] + self.probe_backlash_y + x3, y3 = self.point_coords[2][0] - self.probe_backlash_x, self.point_coords[2][1] + c = (x1-x2)**2 + (y1-y2)**2 + a = (x2-x3)**2 + (y2-y3)**2 + b = (x3-x1)**2 + (y3-y1)**2 + s = 2*(a*b + b*c + c*a) - (a*a + b*b + c*c) + px = (a*(b+c-a)*x1 + b*(c+a-b)*x2 + c*(a+b-c)*x3) / s + py = (a*(b+c-a)*y1 + b*(c+a-b)*y2 + c*(a+b-c)*y3) / s + ar = a**0.5 + br = b**0.5 + cr = c**0.5 + r = ar*br*cr / ((ar+br+cr)*(-ar+br+cr)*(ar-br+cr)*(ar+br-cr))**0.5 + self.tooling_radius_1 = r + gcmd.respond_info('radius_tooling_1= %s,(backlash_y and X) centr_tool(%s;%s)' % ( + self.tooling_radius_1, px, py)) + return px, py, r + cmd_GET_RADIUS_TOOLING_help = "command for get the tooling radius from measuring points." + + def get_radius_2(self, gcmd): + # calculate radius whis probe_backlash_y_2 + x1, y1 = self.point_coords[1][0] + self.probe_backlash_x, self.point_coords[1][1] + x2, y2 = self.point_coords[0][0], self.point_coords[0][1] + self.probe_backlash_y_2 + x3, y3 = self.point_coords[2][0] - self.probe_backlash_x, self.point_coords[2][1] + c = (x1-x2)**2 + (y1-y2)**2 + a = (x2-x3)**2 + (y2-y3)**2 + b = (x3-x1)**2 + (y3-y1)**2 + s = 2*(a*b + b*c + c*a) - (a*a + b*b + c*c) + px = (a*(b+c-a)*x1 + b*(c+a-b)*x2 + c*(a+b-c)*x3) / s + py = (a*(b+c-a)*y1 + b*(c+a-b)*y2 + c*(a+b-c)*y3) / s + ar = a**0.5 + br = b**0.5 + cr = c**0.5 + r = ar*br*cr / ((ar+br+cr)*(-ar+br+cr)*(ar-br+cr)*(ar+br-cr))**0.5 + self.tooling_radius_2 = r + gcmd.respond_info('radius_tooling_2= %s,(backlash_y_2 and X) centr_tool(%s;%s)' % ( + self.tooling_radius_2, px, py)) + return px, py, r + def cmd_SAVE_WCS_CALC_POINT(self, gcmd): point_idx = gcmd.get_int('POINT', 0) coords = gcmd.get('COORDS', None) @@ -148,7 +222,13 @@ def cmd_SET_AUTO_WCS(self, gcmd): def get_status(self, eventtime=None): return { - "wcs": self.wcs + "wcs": self.wcs, + "probe_backlash_x": self.probe_backlash_x, + "probe_backlash_y": self.probe_backlash_y, + "probe_backlash_y_2": self.probe_backlash_y_2, + 'tooling_radius': self.tooling_radius, + 'tooling_radius_1': self.tooling_radius_1, + 'tooling_radius_2': self.tooling_radius_2 } def load_config(config): diff --git a/stereotech_config/probe.cfg b/stereotech_config/probe.cfg index 847a4ddfa66e..42432ec1be83 100644 --- a/stereotech_config/probe.cfg +++ b/stereotech_config/probe.cfg @@ -358,6 +358,26 @@ gcode: MOVE_TO_AUTO_WCS {% endif %} +[gcode_macro SAVE_PROBE_BACKLASH] +description: this macros save the sensor backlash. +gcode: + {% set probe_backlash_x = printer.auto_wcs.probe_backlash_x %} + {% set probe_backlash_y = printer.auto_wcs.probe_backlash_y %} + {% set probe_backlash_y_2 = printer.auto_wcs.probe_backlash_y_2 %} + SAVE_VARIABLE VARIABLE=probe_backlash_x VALUE={probe_backlash_x} + SAVE_VARIABLE VARIABLE=probe_backlash_y VALUE={probe_backlash_y} + SAVE_VARIABLE VARIABLE=probe_backlash_y_2 VALUE={probe_backlash_y_2} + {action_respond_info('save probe_backlash_x=%s' % probe_backlash_x)} + {action_respond_info('save probe_backlash_y=%s' % probe_backlash_y)} + {action_respond_info('save probe_backlash_y_2=%s' % probe_backlash_y_2)} + +[gcode_macro SAVE_RADIUS_TOOL] +description: this macros save the radius tool. +gcode: + SAVE_VARIABLE VARIABLE=tooling_radius VALUE={printer.auto_wcs.tooling_radius} + SAVE_VARIABLE VARIABLE=tooling_radius_1 VALUE={printer.auto_wcs.tooling_radius_1} + SAVE_VARIABLE VARIABLE=tooling_radius_2 VALUE={printer.auto_wcs.tooling_radius_2} + [gcode_macro MOVE_TO_AUTO_WCS] gcode: {% set set_xy = params.XY|default(0) %} @@ -455,6 +475,10 @@ gcode: TOOL_RADIUS ; move and probing for measuring the wcs_2_y AUTO_BASEMENT_WCS_TWO_Y_MOVE + ; set wcs_2_y + ADJUST_BASEMENT_WCS WCS=3 + ; move and probing for measuring the wcs_2_z + AUTO_BASEMENT_WCS_TWO_Z_MOVE {% else %} ; move for measuring wcs_1_z AUTO_BASEMENT_WCS_MOVE @@ -504,9 +528,13 @@ gcode: G0 X{x} Y{y - 20} F3600 {% else %} SET_GCODE_VARIABLE MACRO=AUTO_BASEMENT_WCS_TWO_Y_MOVE VARIABLE=length_is_enough VALUE=0 - {action_raise_error("Tool length not enough for calculate wcs_2_y!")} + SEND_ERROR {% endif %} +[gcode_macro SEND_ERROR] +gcode: + {action_raise_error("Tool length not enough for calculate wcs_2_y!")} + [gcode_macro TOOL_RADIUS] description: moved to measure tool radius and calculate it. gcode: @@ -515,6 +543,7 @@ gcode: SET_AUTO_WCS_POINT_RADIUS POINT={i} {% endfor %} GET_RADIUS_TOOLING + SAVE_RADIUS_TOOL [gcode_macro AUTO_BASEMENT_MOVE_MEASURE_RADIUS] description: measuring radius for geting wcs_2_z. @@ -567,8 +596,8 @@ gcode: {% set y = wcs_offsets[1] - offsets[1] %} {% set z = wcs_offsets[2] + offsets[2] %} G0 A90 F3600 - G0 X{x} Y{y + 5} F3600 G0 Z{z + (radius + 10)} F3600 + G0 X{x} Y{y + 5} F3600 PROBE G0 Z150 F3600 @@ -589,8 +618,13 @@ gcode: {% if wcs == 0 %} ; Mode SPIRAL-FULL {% if probe_sensor_version %} - ; apply measuring for the set wcs_2_y - G10 L2 P3 Y{y + probe_backlash_y} + ; apply measuring for the set wcs_2_z radius. + {% if printer['gcode_macro AUTO_BASEMENT_WCS_TWO_Y_MOVE'].length_is_enough|int %} + {% set radius = printer.auto_wcs.tooling_radius %} + G10 L2 P3 Z{z - radius} + {% else %} + {action_respond_info('Error, tool length not enough for calculate wcs_2_y!')} + {% endif %} {% else %} ; apply measuring for the set wcs_1_z and wcs_2_y. G10 L2 P2 Z{z} @@ -607,7 +641,7 @@ gcode: G10 L2 P2 Z{z} G10 L2 P3 Y{old_y - (z - old_z)} {% elif wcs == 3 %} - ; {% set radius = printer.auto_wcs.tooling_radius %} - ; apply measuring for the set wcs_2_z radius. - ; G10 L2 P3 Z{z - radius} + ; crutch! for the support different version sensor + ; apply measuring for the set wcs_2_y + G10 L2 P3 Y{y + probe_backlash_y} {% endif %} diff --git a/stereotech_config/probe_2.cfg b/stereotech_config/probe_2.cfg index 49fc78d092d4..c9738705abb0 100644 --- a/stereotech_config/probe_2.cfg +++ b/stereotech_config/probe_2.cfg @@ -330,6 +330,26 @@ gcode: MOVE_TO_AUTO_WCS {% endif %} +[gcode_macro SAVE_PROBE_BACKLASH] +description: this macros save the sensor backlash. +gcode: + {% set probe_backlash_x = printer.auto_wcs.probe_backlash_x %} + {% set probe_backlash_y = printer.auto_wcs.probe_backlash_y %} + {% set probe_backlash_y_2 = printer.auto_wcs.probe_backlash_y_2 %} + SAVE_VARIABLE VARIABLE=probe_backlash_x VALUE={probe_backlash_x} + SAVE_VARIABLE VARIABLE=probe_backlash_y VALUE={probe_backlash_y} + SAVE_VARIABLE VARIABLE=probe_backlash_y_2 VALUE={probe_backlash_y_2} + {action_respond_info('probe_backlash_x=%s' % probe_backlash_x)} + {action_respond_info('probe_backlash_y=%s' % probe_backlash_y)} + {action_respond_info('probe_backlash_y_2=%s' % probe_backlash_y_2)} + +[gcode_macro SAVE_RADIUS_TOOL] +description: this macros save the radius tool. +gcode: + SAVE_VARIABLE VARIABLE=tooling_radius VALUE={printer.auto_wcs.tooling_radius} + SAVE_VARIABLE VARIABLE=tooling_radius_1 VALUE={printer.auto_wcs.tooling_radius_1} + SAVE_VARIABLE VARIABLE=tooling_radius_2 VALUE={printer.auto_wcs.tooling_radius_2} + [gcode_macro MOVE_TO_AUTO_WCS] gcode: {% set set_xy = params.XY|default(0) %} @@ -425,6 +445,10 @@ gcode: TOOL_RADIUS ; move and probing for measuring the wcs_2_y AUTO_BASEMENT_WCS_TWO_Y_MOVE + ; set wcs_2_y + ADJUST_BASEMENT_WCS WCS=3 + ; move and probing for measuring the wcs_2_z + AUTO_BASEMENT_WCS_TWO_Z_MOVE {% else %} PROBE {% endif %} @@ -469,9 +493,13 @@ gcode: G0 X{x} Y{y - 20} F3600 {% else %} SET_GCODE_VARIABLE MACRO=AUTO_BASEMENT_WCS_TWO_Y_MOVE VARIABLE=length_is_enough VALUE=0 - {action_raise_error("Tool length not enough for calculate wcs_2_y!")} + SEND_ERROR {% endif %} +[gcode_macro SEND_ERROR] +gcode: + {action_raise_error("Tool length not enough for calculate wcs_2_y!")} + [gcode_macro TOOL_RADIUS] description: moved to measure tool radius and calculate it. gcode: @@ -480,6 +508,7 @@ gcode: SET_AUTO_WCS_POINT_RADIUS POINT={i} {% endfor %} GET_RADIUS_TOOLING + SAVE_RADIUS_TOOL [gcode_macro AUTO_BASEMENT_MOVE_MEASURE_RADIUS] description: measuring radius for geting wcs_2_z. @@ -532,8 +561,8 @@ gcode: {% set y = wcs_offsets[1] - offsets[1] %} {% set z = wcs_offsets[2] + offsets[2] %} G0 A90 F3600 - G0 X{x} Y{y + 5} F3600 G0 Z{z + (radius + 10)} F3600 + G0 X{x} Y{y + 5} F3600 PROBE G0 Z150 F3600 @@ -552,21 +581,25 @@ gcode: {% set probe_backlash_y = printer.save_variables.variables.probe_backlash_y|default(0.0)|float %} {% if wcs == 0 %} ; Mode SPIRAL-FULL - ; apply measuring for the set wcs_2_y. - G10 L2 P3 Y{y + probe_backlash_y} + ; apply measuring for the set wcs_2_z radius. + {% if printer['gcode_macro AUTO_BASEMENT_WCS_TWO_Y_MOVE'].length_is_enough|int %} + {% set radius = printer.auto_wcs.tooling_radius %} + G10 L2 P3 Z{z - radius} + {% else %} + {action_respond_info('Error, tool length not enough for calculate wcs_2_y!')} + {% endif %} {% elif wcs == 1 %} ; Mode SPIRAL ; apply measuring for the set wcs_2_y and wcs_1_z(raw). G10 L2 P3 Y{y} G10 L2 P2 Z{old_z - (y - old_y)} {% elif wcs == 2 %} - ; Mode SPIRAL-FULL ; crutch! for the support different version sensor ; set wcs_1_z and wcs_2_y(raw). G10 L2 P2 Z{z} G10 L2 P3 Y{old_y - (z - old_z)} {% elif wcs == 3 %} - ; {% set radius = printer.auto_wcs.tooling_radius %} - ; apply measuring for the set wcs_2_z radius. - ; G10 L2 P3 Z{z - radius} + ; crutch! for the support different version sensor + ; apply measuring for the set wcs_2_y + G10 L2 P3 Y{y + probe_backlash_y} {% endif %}