STEAPP-541: added load/save data about the sensor backlash. (#134)

* STEAPP-541: added load/save the sensor backlash. Added calculate backlash in to main logic * STEAPP-541: added calculate backlash in to main logic * STEAPP-541: fixed bug in to apply radius for calculate wcs_2_z. * STEAPP-541: Removed unnecessary save radius and backlash sensor commands because SAVE_VARIABLES does that.
stereotech · Jun 22, 2023 · f860eeb · f860eeb
1 parent e60a8de
commit f860eeb
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 46 deletions.
diff --git a/klippy/extras/auto_wcs.py b/klippy/extras/auto_wcs.py
@@ -1,4 +1,5 @@
 import math
+import logging
 
 
 RAD_TO_DEG = 57.295779513
@@ -42,6 +43,12 @@ def __init__(self, config):
         self.gcode.register_command(
             'SET_AUTO_WCS', self.cmd_SET_AUTO_WCS,
             desc=self.cmd_CALC_WCS_PARAMS_help)
+        self.gcode.register_command(
+            'GET_RADIUS_TOOLING', self.cmd_GET_RADIUS_TOOLING,
+            desc=self.cmd_GET_RADIUS_TOOLING_help)
+        self.gcode.register_command(
+            'SET_PROBE_BACKLASH', self.cmd_SET_PROBE_BACKLASH,
+            desc=self.cmd_SET_PROBE_BACKLASH_help)
 
     def _calc_wcs_old_sensor(self, thickness, adj, gcmd):
         thickness = thickness / 2.0
@@ -136,7 +143,6 @@ def get_radius_1(self, gcmd):
         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
@@ -176,7 +182,6 @@ def cmd_SAVE_WCS_CALC_POINT(self, gcmd):
 
     cmd_SAVE_WCS_CALC_POINT_help = "Save point for WCS calculation"
 
-
     def cmd_CALC_WCS_PARAMS(self, gcmd):
         #todo: get thickness default 10
         thickness =  gcmd.get_float('THICKNESS', 10.)
@@ -185,6 +190,7 @@ def cmd_CALC_WCS_PARAMS(self, gcmd):
         if sensor_version:
             x, y, z = self._calc_wcs_new_sensor(thickness, adjustment_coeff, gcmd)
             x2, y2, z2 = self._calc_wcs_2_new_sensor(thickness, adjustment_coeff, gcmd)
+            self.calculate_probe_backlash(x, y, y2)
             delta_y = y - y2
             delta_z = z - z2
             avg_delta = (delta_y + delta_z) / 2.0
@@ -218,7 +224,15 @@ def cmd_SET_AUTO_WCS(self, gcmd):
             for axis, coord in enumerate(coords):
                 self.wcs[point_idx][axis] = coord
 
-    cmd_CALC_WCS_PARAMS_help = "Perform WCS calculation"
+    def cmd_SET_PROBE_BACKLASH(self, gcmd):
+        self.probe_backlash_x = gcmd.get_float('BACKLASH_X', self.probe_backlash_x)
+        self.probe_backlash_y = gcmd.get_float('BACKLASH_Y', self.probe_backlash_y)
+        self.probe_backlash_y_2 = gcmd.get_float('BACKLASH_Y_2', self.probe_backlash_y_2)
+        logging.info(
+            'Probe backlash is set:\nprobe_backlash_x=%f, probe_backlash_y=%f, probe_backlash_y_2=%f' % (
+                self.probe_backlash_x, self.probe_backlash_y, self.probe_backlash_y_2)
+            )
+    cmd_SET_PROBE_BACKLASH_help = "Set the sensor backlash for the current sensor."
 
     def get_status(self, eventtime=None):
         return {

diff --git a/stereotech_config/probe.cfg b/stereotech_config/probe.cfg
@@ -358,26 +358,6 @@ 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) %}
@@ -540,7 +520,6 @@ 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.
@@ -609,7 +588,7 @@ gcode:
     {% set old_z = wcs_0[2] %}
     {% set old_y = wcs_1[1] %}
     {% set probe_sensor_version = printer.save_variables.variables.probe_sensor_version|default(0)|int %}
-    {% set probe_backlash_y = printer.save_variables.variables.probe_backlash_y|default(0.0)|float %}
+    {% set probe_backlash_y = printer.auto_wcs.probe_backlash_y|default(0.0)|float %}
     {% if wcs == 0 %}
         ; Mode SPIRAL-FULL
         {% if probe_sensor_version %}

diff --git a/stereotech_config/probe_2.cfg b/stereotech_config/probe_2.cfg
@@ -330,25 +330,6 @@ 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:
@@ -506,7 +487,6 @@ 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.
@@ -574,7 +554,7 @@ gcode:
     {% set wcs_1 = printer.gcode_move.wcs_offsets[4] %}
     {% set old_z = wcs_0[2] %}
     {% set old_y = wcs_1[1] %}
-    {% set probe_backlash_y = printer.save_variables.variables.probe_backlash_y|default(0.0)|float %}
+    {% set probe_backlash_y = printer.auto_wcs.probe_backlash_y|default(0.0)|float %}
     {% if wcs == 0 %}
         ; Mode SPIRAL-FULL
         ; apply measuring for the set wcs_2_z radius.

diff --git a/stereotech_config/variables.cfg b/stereotech_config/variables.cfg
@@ -31,6 +31,7 @@ gcode:
                 SKEW_PROFILE CHANGE={key} XY={svv[key ~ '_xy_skew']} XZ={svv[key ~ '_xz_skew']} YZ={svv[key ~ '_yz_skew']}
             {% endif %}
         {% endfor %}
+        SET_PROBE_BACKLASH BACKLASH_X={svv.probe_backlash_x|default(0.0)|float} BACKLASH_Y={svv.probe_backlash_y|default(0.0)|float} BACKLASH_Y_2={svv.probe_backlash_y_2|default(0.0)|float}
     {% endif %}
     {% if printer["filament_motion_sensor extruder_sensor"] %}
         SET_FILAMENT_SENSOR SENSOR=extruder_sensor ENABLE={svv['extruder_sensor']|default(1)}
@@ -72,4 +73,8 @@ gcode:
         SAVE_VARIABLE VARIABLE=probe_offset_x VALUE={printer.probe.offsets[0]}
         SAVE_VARIABLE VARIABLE=probe_offset_y VALUE={printer.probe.offsets[1]}
         SAVE_VARIABLE VARIABLE=probe_offset_z VALUE={printer.probe.offsets[2]}
+
+        SAVE_VARIABLE VARIABLE=probe_backlash_x VALUE={printer.auto_wcs.probe_backlash_x}
+        SAVE_VARIABLE VARIABLE=probe_backlash_y VALUE={printer.auto_wcs.probe_backlash_y}
+        SAVE_VARIABLE VARIABLE=probe_backlash_y_2 VALUE={printer.auto_wcs.probe_backlash_y_2}
     {% endif %}