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

klippy/extras/auto_wcs.py

@@ -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"

klippy/extras/skew_correction.py

@@ -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",
@@ -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.],
@@ -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
@@ -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
@@ -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])
@@ -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])
@@ -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
@@ -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)
 
@@ -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):
@@ -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')
@@ -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):

stereotech_config/print_macros.cfg

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