Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

STEAPP-514: added calculate wcs_2_z and radius only whis baclash_y, added save raadius. #132

Merged
merged 4 commits into from
May 29, 2023
+163 −16
Merged

STEAPP-514: added calculate wcs_2_z and radius only whis baclash_y, added save raadius. #132

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
2aea5f5
STEAPP: added calculate wcs_2_z, save radius, calculate radius only w…
SokolovJek May 25, 2023
961d6d0
STEAPP-514: WIP added calculate radius only whis baclash_y.
SokolovJek May 25, 2023
12fad39
STEAPP-514: Added a condition that does not set wcs_2_z after an error.
SokolovJek May 26, 2023
f923307
Merge branch 'develop' into STEAPP-514
frylock34 May 29, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
82 changes: 81 additions & 1 deletion klippy/extras/auto_wcs.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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):
Expand Down
48 changes: 41 additions & 7 deletions stereotech_config/probe.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) %}
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand All @@ -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.
Expand Down Expand Up @@ -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

Expand All @@ -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}
Expand All @@ -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 %}
49 changes: 41 additions & 8 deletions stereotech_config/probe_2.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) %}
Expand Down Expand Up @@ -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 %}
Expand Down Expand Up @@ -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:
Expand All @@ -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.
Expand Down Expand Up @@ -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

Expand All @@ -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 %}