Merge pull request #243 from stereotech/STEAPP-734

STEAPP-734: Added recursive offset measurement for the A-axis

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):

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

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

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

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:

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

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

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