Skip to content

Commit

Permalink
STEAPP-600: WIP: added new algorithm to calculate wcs for the SPIRAL …
Browse files Browse the repository at this point in the history 
…mode. (#154)

* STEAPP-555: added new comand and move for calculate skew beetwen two points the X axis.

* STEAPP-556: added commands for set and apply the correct accentricity wcs_1_x/y.

* STEAPP-556: added commands for set and apply the offset for correcting the eccentricity.

* STEAPP-556: fixed bug, and added condition for checking length the tool
 before measure skew for the axis X(where use points only wcs_1).

* STEAPP-581: added new logic for apply skew compensation

* STEAPP-581: disabled skew compensation in the CANCEL_PRINT command.

* STEAPP-581: Edited and tested new algorithm.

* STEAPP-581: Edited and tested new algorithm.

* STEAPP-581: Edited and tested new algorithm.

* STEAPP-581: refactoring the code.

* STEAPP-581: added save skew for quickly use this functionality.

* STEAPP-581: added save skew for quickly use this functionality.

* STEAPP-600: added mode calibrate to function for get rough radius.

* STEAPP-600: added new algorithm to calculate wcs for the SPIRAL mode.

* STEAPP-581: refactoring the code.

* STEAPP-600: disabled skew compensation and apply eccentricity

---------

Co-authored-by: Ilya Gushchin <[email protected]>
  • Loading branch information
@SokolovJek @frylock34
SokolovJek and frylock34 authored Aug 8, 2023
1 parent 274ab12 commit 5dd798e
Show file tree
Hide file tree
Showing 5 changed files with 475 additions and 151 deletions.
17 changes: 11 additions & 6 deletions klippy/extras/auto_wcs.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -180,17 +180,22 @@ def cmd_CALC_WCS_TOOL(self, gcmd):

cmd_GET_RADIUS_TOOLING_help = "command for get the tooling radius from measuring points."
def cmd_GET_RADIUS_TOOLING(self, gcmd):
advance = gcmd.get_int('ADVANCE', 0)
if not advance:
rough = gcmd.get_int('ROUGH', 0)
if not rough:
self.tooling_radius = self.get_radius(gcmd)
self.tooling_radius_1 = self.get_radius_1(gcmd)
self.tooling_radius_2 = self.get_radius_2(gcmd)
else:
# if needed calculate advance radius
# if needed calculate rough radius
mode = gcmd.get('MODE')
gcode_move = self.printer.lookup_object('gcode_move')
y2 = self.point_coords[0][1] + self.probe_backlash_y
self.tooling_radius = gcode_move.wcs_offsets[1][1] - y2
gcmd.respond_info('advance radius_tooling= %s' % self.tooling_radius)
if mode == 'full':
y = self.point_coords[0][1] + self.probe_backlash_y
self.tooling_radius = gcode_move.wcs_offsets[3][1] - y
elif mode == 'spiral':
z = self.point_coords[0][2]
self.tooling_radius = z - gcode_move.wcs_offsets[4][2]
gcmd.respond_info('rough radius_tooling= %s' % self.tooling_radius)
return self.tooling_radius

cmd_SAVE_WCS_CALC_POINT_help = "Save point for WCS calculation"
Expand Down
130 changes: 95 additions & 35 deletions klippy/extras/skew_correction.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,12 +43,13 @@ def __init__(self, config):
self.xy_factor = 0.
self.xz_factor = 0.
self.yz_factor = 0.
self.apply_skew = False
self.current_profile = None
self.skew_profiles = {}
# Fetch stored profiles from Config
self._load_storage(config)
self.printer.register_event_handler("klippy:connect",
self._handle_connect)
self.printer.register_event_handler("klippy:ready",
self._handle_ready)
self.printer.register_event_handler("gcode_move:change_wcs_lists",
self._change_wcs_lists)
# self.printer.register_event_handler("gcode_move:change_current_wcs",
Expand All @@ -69,6 +70,9 @@ def __init__(self, config):
gcode.register_command('CALC_SKEW_COMPENSATION',
self.cmd_CALC_SKEW_COMPENSATION,
desc=self.cmd_CALC_SKEW_COMPENSATION_help)
gcode.register_command('CALC_SKEW_COMPENSATION_WCS',
self.cmd_CALC_SKEW_COMPENSATION_WCS,
desc=self.cmd_CALC_SKEW_COMPENSATION_WCS_help)
self.point_coords = [
[0., 0., 0.],
[0., 0., 0.],
Expand All @@ -83,9 +87,9 @@ def __init__(self, config):
[0., 0., 0.]
]
self.next_transform = None
self.start_z_point = 0.

def _handle_connect(self):

def _handle_ready(self):
kin = self.printer.lookup_object('toolhead').get_kinematics()
self.axes_min = kin.axes_min
self.axes_max = kin.axes_max
Expand Down Expand Up @@ -135,6 +139,7 @@ def cmd_CALC_SKEW_COMPENSATION(self, gcmd):
"""
factors = ['XY', 'XZ', 'YZ']
factor_name = gcmd.get('FACTOR').upper()
msg = gcmd.get('MSG', '')
if factor_name in factors:
if factor_name == factors[0]:
# xy_factor
Expand All @@ -144,6 +149,7 @@ def cmd_CALC_SKEW_COMPENSATION(self, gcmd):
b_point = get_point(self.point_coords[0], self.point_coords[1])
c_point = list(b_point)
c_point[0] = c_point[0] + 50.
diff_points = b_point[0] - a_point[0]
bc = length_side(b_point[0], b_point[1], c_point[0], c_point[1])
bd = length_side(b_point[0], b_point[1], d_point[0], d_point[1])
ac = length_side(a_point[0], a_point[1], c_point[0], c_point[1])
Expand All @@ -155,6 +161,7 @@ def cmd_CALC_SKEW_COMPENSATION(self, gcmd):
b_point = get_point(self.point_coords[0], self.point_coords[1])
c_point = list(b_point)
c_point[0] = c_point[0] + 50
diff_points = b_point[0] - a_point[0]
bc = length_side(b_point[0], b_point[2], c_point[0], c_point[2])
bd = length_side(b_point[0], b_point[2], d_point[0], d_point[2])
ac = length_side(a_point[0], a_point[2], c_point[0], c_point[2])
Expand All @@ -166,20 +173,70 @@ def cmd_CALC_SKEW_COMPENSATION(self, gcmd):
b_point = get_point(self.point_coords[1], self.point_coords[2])
c_point = list(b_point)
c_point[1] = c_point[1] - 50
diff_points = b_point[1] - a_point[1]
bc = length_side(b_point[1], b_point[2], c_point[1], c_point[2])
bd = length_side(b_point[1], b_point[2], d_point[1], d_point[2])
ac = length_side(a_point[1], a_point[2], c_point[1], c_point[2])
factor_value = float("%.4f" % calc_skew_factor(ac, bd, bc))
factor_name = factor_name.lower() + '_factor'
setattr(self, factor_name, factor_value)
out = "Calculated skew compensation %s: %.6f radians, %.2f degrees\n" % (
factor_name, factor_value, math.degrees(factor_value))
out = "Calculated %s skew compensation %s: %.6f radians, %.2f degrees, difference beetwen points=%f" % (
msg, factor_name, factor_value, math.degrees(factor_value), diff_points)
gcmd.respond_info(out)
else:
raise gcmd.error(
"Error! Factor name %s not in list factors['XY', 'XZ', 'YZ']" % (factor_name))
cmd_CALC_SKEW_COMPENSATION_help = "Calculate skew compensation."

cmd_CALC_SKEW_COMPENSATION_WCS_help = "Calculate skew compensation by wcs."
def cmd_CALC_SKEW_COMPENSATION_WCS(self, gcmd):
factors = ['XY', 'XZ', 'YZ']
factor_name = gcmd.get('FACTOR').upper()
factor_name = factor_name
wcs_list = self.gcode_move.wcs_offsets
if factor_name in factors:
if factor_name == factors[0]:
# xy_factor
a_point = wcs_list[2]
d_point = list(a_point)
d_point[0] = d_point[0] + 50
b_point = wcs_list[4]
c_point = list(b_point)
c_point[0] = c_point[0] + 50
# diff_points = b_point[0] - a_point[0]
bc = length_side(b_point[0], b_point[1], c_point[0], c_point[1])
bd = length_side(b_point[0], b_point[1], d_point[0], d_point[1])
ac = length_side(a_point[0], a_point[1], c_point[0], c_point[1])
elif factor_name == factors[1]:
# xz_factor
a_point = wcs_list[3]
d_point = list(a_point)
d_point[0] = d_point[0] + 50.
b_point = wcs_list[1]
c_point = list(b_point)
c_point[0] = c_point[0] + 50.
# diff_points = b_point[0] - a_point[0]
bc = length_side(b_point[0], b_point[2], c_point[0], c_point[2])
bd = length_side(b_point[0], b_point[2], d_point[0], d_point[2])
ac = length_side(a_point[0], a_point[2], c_point[0], c_point[2])
else:
# yz_factor
a_point = wcs_list[1]
d_point = list(a_point)
d_point[1] = d_point[1] + 50
b_point = wcs_list[3]
c_point = list(b_point)
c_point[1] = c_point[1] + 50
bc = length_side(b_point[1], b_point[2], c_point[1], c_point[2])
bd = length_side(b_point[1], b_point[2], d_point[1], d_point[2])
ac = length_side(a_point[1], a_point[2], c_point[1], c_point[2])
factor_value = float("%.4f" % calc_skew_factor(ac, bd, bc))
factor_name = factor_name.lower() + '_factor'
setattr(self, factor_name, factor_value)
out = "Calculated skew compensation %s: %.6f radians, %.2f degrees" % (
factor_name, factor_value, math.degrees(factor_value))
gcmd.respond_info(out)

def _load_storage(self, config):
stored_profs = config.get_prefix_sections(self.name)
# Remove primary skew_correction section, as it is not a stored profile
Expand All @@ -195,39 +252,44 @@ def _load_storage(self, config):

def calc_skew(self, pos):
newpos = list(pos)
# pos_z = pos[2] - self.wcs_list[self.current_wcs + 2][2]
# used middle point, an alternative to getting the actual z coordinate in wcs.
pos_z = pos[2] - self.start_z_point
# newpos[0] = pos[0] - pos[1] * self.xy_factor \
# - pos[2] * (self.xz_factor - (self.xy_factor * self.yz_factor))
# removed self.xy_factor from the formula for a cleaner and easier compensation application.
newpos[0] = pos[0] - pos_z * self.xz_factor
newpos[1] = pos[1] - pos_z * (self.yz_factor * -1)
return newpos
if self.gcode_move.current_wcs == 2:
newpos[0] = pos[0] + ((self.wcs_list[2][1] - pos[1]) * (self.xy_factor * -1 ))
return newpos
elif self.gcode_move.current_wcs == 1:
newpos[0] = pos[0] + ((self.wcs_list[1][2] - pos[2]) * (self.xz_factor * -1 ))
newpos[1] = pos[1] + ((self.wcs_list[1][2] - pos[2]) * (self.yz_factor))
return newpos
else:
return pos

def calc_unskew(self, pos):
newpos = list(pos)
# pos_z = pos[2] - self.wcs_list[self.current_wcs + 2][2]
pos_z = pos[2] - self.start_z_point
# newpos[0] = pos[0] + pos[1] * self.xy_factor \
# + pos[2] * self.xz_factor
newpos[0] = pos[0] + pos_z * self.xz_factor
newpos[1] = pos[1] + pos_z * (self.yz_factor * -1)
return newpos
if self.gcode_move.current_wcs == 2:
newpos[0] = pos[0] - ((self.wcs_list[2][1] - pos[1]) * (self.xy_factor * -1 ))
return newpos
elif self.gcode_move.current_wcs == 1:
newpos[0] = pos[0] - ((self.wcs_list[1][2] - pos[2]) * (self.xz_factor * -1 ))
newpos[1] = pos[1] - ((self.wcs_list[1][2] - pos[2]) * (self.yz_factor))
return newpos
else:
return pos

def get_position(self):
if not self.enabled:
if self.apply_skew and self.enabled:
return self.calc_unskew(self.next_transform.get_position())
else:
return self.next_transform.get_position()
return self.calc_unskew(self.next_transform.get_position())

def move(self, newpos, speed):
old_pos = self.next_transform.get_position()
axes_d = [old_pos[i] - newpos[i] for i in
(0, 1, 2, 3, 4, 5)]
move_d = math.sqrt(sum([d * d for d in axes_d[:5]]))
if not self.enabled or move_d < .000000001:
self.apply_skew = False
self.next_transform.move(newpos, speed)
return
self.apply_skew = True
corrected_pos = self.calc_skew(newpos)
self.next_transform.move(corrected_pos, speed)

Expand Down Expand Up @@ -299,16 +361,11 @@ def cmd_SKEW_PROFILE(self, gcmd):

def load_profile(self, prof_name):
"""
load profile and set neaded start point Z.
load profile
"""
self.current_profile = prof_name
if prof_name == 'module_3d':
self.start_z_point = self.wcs_list[0]
else:
# get middle point, an alternative to getting the actual wcs index.
self.start_z_point = (self.wcs_list[3][2] + self.wcs_list[4][2]) / 2.
profile = self.skew_profiles.get(prof_name)
if profile is not None:
self.current_profile = prof_name
self._update_skew(profile['xy_skew'], profile['xz_skew'], profile['yz_skew'])

def save_profile(self, prof_name):
Expand All @@ -331,14 +388,17 @@ def change_profile(self, prof_name, gcmd):
"""
change profile factors but don't change config
"""
xy_factor = gcmd.get_float('XY', 0.)
xz_factor = gcmd.get_float('XZ', 0.)
yz_factor = gcmd.get_float('YZ', 0.)
xy_factor = gcmd.get_float('XY', self.skew_profiles[prof_name]['xy_skew'])
xz_factor = gcmd.get_float('XZ', self.skew_profiles[prof_name]['xz_skew'])
yz_factor = gcmd.get_float('YZ', self.skew_profiles[prof_name]['yz_skew'])
self.skew_profiles[prof_name] = {
'xy_skew': xy_factor,
'xz_skew': xz_factor,
'yz_skew': yz_factor
}
gcmd.respond_info(
"Changed skew_profile=%s: xy_skew=%s; xz_skew=%s; yz_skew=%s." % (
prof_name, xy_factor, xz_factor, yz_factor))

def remove_profile(self, prof_name):
configfile = self.printer.lookup_object('configfile')
Expand All @@ -358,7 +418,7 @@ def get_status(self, eventtime=None):
'skew_profiles': self.skew_profiles,
'wcs_list': self.wcs_list,
'current_skew_profile': self.current_profile,
'start_z_point': self.start_z_point
'self.point_coords': self.point_coords
}

def load_config(config):
Expand Down
10 changes: 5 additions & 5 deletions stereotech_config/print_macros.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ gcode:
{% else %}
{% set x = params.X|default(200) %}
{% set y = params.Y|default(10) %}
{% endif %}
{% endif %}
{% set z = params.Z|default(50)|float %}
{% set e = params.E|default(3) %}
{% set turn_off_extruders = params.TURN_OFF_EXTRUDERS|default(1)|int %}
Expand Down Expand Up @@ -53,8 +53,8 @@ gcode:
{% else %}
;FIVE AXIS COMPENSATION OFF
;B_AXIS_COMPENSATION_VARS ENABLE=0
;SET_SKEW ENABLE=0
RADIAL_SPEED_COMPENSATION ENABLE=0
SET_SKEW ENABLE=0
RADIAL_SPEED_COMPENSATION ENABLE=0
{% endif %}
{% endif %}
G54
Expand Down Expand Up @@ -154,7 +154,7 @@ gcode:
{% else %}
;FIVE AXIS COMPENSATION OFF
;B_AXIS_COMPENSATION_VARS ENABLE=0
;SET_SKEW ENABLE=0
SET_SKEW ENABLE=0
RADIAL_SPEED_COMPENSATION ENABLE=0
{% endif %}
{% endif %}
Expand Down Expand Up @@ -257,7 +257,7 @@ gcode:
;SET_SKEW ENABLE=0
{% else %}
;FIVE AXIS COMPENSATION OFF
;SET_SKEW ENABLE=0
SET_SKEW ENABLE=0
;B_AXIS_COMPENSATION_VARS ENABLE=0
RADIAL_SPEED_COMPENSATION ENABLE=0
{% endif %}
Expand Down
Loading

0 comments on commit 5dd798e

Please sign in to comment.