STEAPP-636: PROBE(step-2) Fixed a bug in the tool radius calculation …

…algorithm for SPIRAL mode. And removed unnecessary functions for calculation radius for the FULL mode. (#163) * STEAPP-599: Moved the same macros to a separate configю * STEAPP-599: renamed configs, set logical names. * STEAPP-599: renamed configs, set logical names. * STEAPP-599: new names have been added to configs. * STEAPP-599: added .png image visual how as guide. * STEAPP-599: added descriptions. * STEAPP-636: error detected, incorrect radius calculation * STEAPP-636: Fixed a bug in the tool radius calculation algorithm. * STEAPP-636: Removed functions additional calculations radius for the full mode. * STEAPP-636: added new config files the autocalibrate --------- Co-authored-by: sokolovjek <[email protected]>
stereotech · Aug 31, 2023 · 170dcd1 · 170dcd1
1 parent 0042d43
commit 170dcd1
Show file tree

Hide file tree

Showing 17 changed files with 607 additions and 1,248 deletions.
diff --git a/HTE530-5-4-22.cfg b/HTE530-5-4-22.cfg
@@ -15,7 +15,8 @@ path: /home/ste/uploads
 [include stereotech_config/chamber_2.cfg]
 
 [include stereotech_config/printhead.cfg]
-[include stereotech_config/probe.cfg]
+[include stereotech_config/probe_main.cfg]
+[include stereotech_config/probe_DAC_v1.cfg]
 [include stereotech_config/probe_hybrid_printer.cfg]
 
 [include stereotech_config/main_extruder.cfg]

diff --git a/docs/stereotech/calibrate/auto_calibrate_by_teamplate.png b/docs/stereotech/calibrate/auto_calibrate_by_teamplate.png
diff --git a/docs/stereotech/calibrate/auto_calibrate_by_tool.png b/docs/stereotech/calibrate/auto_calibrate_by_tool.png
diff --git a/...stereotech/manual_calibrate_bed_level.txt → .../calibrate/manual_calibrate_bed_level.txt b/...stereotech/manual_calibrate_bed_level.txt → .../calibrate/manual_calibrate_bed_level.txt
diff --git a/...eotech/manual_calibrate_old_teamplate.txt → ...ibrate/manual_calibrate_old_teamplate.txt b/...eotech/manual_calibrate_old_teamplate.txt → ...ibrate/manual_calibrate_old_teamplate.txt
diff --git a/klippy/extras/auto_wcs.py b/klippy/extras/auto_wcs.py
@@ -29,8 +29,6 @@ def __init__(self, config):
         self.probe_backlash_y = 0.
         self.probe_backlash_y_2 = 0.
         self.tooling_radius = 3.
-        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(
@@ -107,8 +105,8 @@ def calculate_probe_backlash(self, x1, y1, y2):
                                                              self.probe_backlash_y,
                                                              self.probe_backlash_y_2))
 
-    def get_radius(self, gcmd):
-        # calculate radius only whis probe_backlash_y
+    def get_radius_full_mode(self, gcmd):
+        # calculate radius only whis probe_backlash_y for FULL mode
         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]
@@ -126,42 +124,24 @@ def get_radius(self, gcmd):
                 radius, centr_x, centr_y))
         return radius
 
-    def get_radius_1(self, gcmd):
-        # calculate radius whis probe_backlash_y and probe_backlash_x
-        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
-        radius = ar*br*cr / ((ar+br+cr)*(-ar+br+cr)*(ar-br+cr)*(ar+br-cr))**0.5
-        gcmd.respond_info('radius_tooling_1= %s,(backlash_y and X) centr_tool(%s;%s)' % (
-            radius, px, py))
-        return radius
-
-    def get_radius_2(self, gcmd):
-        # calculate radius whis probe_backlash_y_2 and probe_backlash_x
-        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
+    def get_radius_spiral_mode(self, gcmd):
+        # calculate radius for SPIARAL mode
+        x1, z1 = self.point_coords[1][0] + self.probe_backlash_x, self.point_coords[1][2]
+        # x2, z2 = self.point_coords[0][0], self.point_coords[0][2] + (self.tooling_radius - 5)
+        x2, z2 = self.point_coords[0][0], self.point_coords[0][2] - self.probe_backlash_x
+        x3, z3 = self.point_coords[2][0] - self.probe_backlash_x, self.point_coords[2][2]
+        c = (x1-x2)**2 + (z1-z2)**2
+        a = (x2-x3)**2 + (z2-z3)**2
+        b = (x3-x1)**2 + (z3-z1)**2
+        s= 2*(a*b + b*c + c*a) - (a*a + b*b + c*c)
+        centr_x = (a*(b+c-a)*x1 + b*(c+a-b)*x2 + c*(a+b-c)*x3) / s
+        centr_y = (a*(b+c-a)*z1 + b*(c+a-b)*z2 + c*(a+b-c)*z3) / s
         ar = a**0.5
         br = b**0.5
         cr = c**0.5
         radius = ar*br*cr / ((ar+br+cr)*(-ar+br+cr)*(ar-br+cr)*(ar+br-cr))**0.5
-        gcmd.respond_info('radius_tooling_2= %s,(backlash_y_2 and X) centr_tool(%s;%s)' % (
-            radius, px, py))
+        gcmd.respond_info('radius_tooling= %s, centr_tool(%s;%s)' % (
+                radius, centr_x, centr_y))
         return radius
 
     cmd_CALC_WCS_TOOL_help = "command for calculate wcs coordinate for SPIRALL-FULL."
@@ -180,13 +160,14 @@ 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):
         rough = gcmd.get_int('ROUGH', 0)
+        mode = gcmd.get('MODE', 'full')
         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)
+            if mode == 'full':
+                self.tooling_radius = self.get_radius_full_mode(gcmd)
+            elif mode == 'spiral':
+                self.tooling_radius = self.get_radius_spiral_mode(gcmd)
         else:
             # if needed calculate rough radius
-            mode = gcmd.get('MODE')
             gcode_move = self.printer.lookup_object('gcode_move')
             if mode == 'full':
                 y = self.point_coords[0][1] + self.probe_backlash_y

diff --git a/stereotech_config/HFE530-5-3-23.cfg b/stereotech_config/HFE530-5-3-23.cfg
@@ -15,7 +15,8 @@ path: /home/ste/uploads
 [include config/chamber_2.cfg]
 
 [include config/printhead.cfg]
-[include config/probe_2.cfg]
+[include config/probe_main.cfg]
+[include config/probe_DAC_v2.cfg]
 [include config/probe_fiber_printer.cfg]
 
 [include config/main_extruder.cfg]

diff --git a/stereotech_config/HFE530-5-4-22.cfg b/stereotech_config/HFE530-5-4-22.cfg
@@ -15,7 +15,8 @@ path: /home/ste/uploads
 [include config/chamber_2.cfg]
 
 [include config/printhead.cfg]
-[include config/probe.cfg]
+[include config/probe_main.cfg]
+[include config/probe_DAC_v1.cfg]
 [include config/probe_hybrid_printer.cfg]
 
 [include config/main_extruder.cfg]

diff --git a/stereotech_config/HFE530-5-8-23.cfg b/stereotech_config/HFE530-5-8-23.cfg
@@ -15,7 +15,8 @@ path: /home/ste/uploads
 [include config/chamber_2.cfg]
 
 [include config/printhead.cfg]
-[include config/probe_2.cfg]
+[include config/probe_main.cfg]
+[include config/probe_DAC_v2.cfg]
 [include config/probe_fiber_printer_3.cfg]
 
 [include config/main_extruder.cfg]

diff --git a/stereotech_config/HFE530-5-C-22.cfg b/stereotech_config/HFE530-5-C-22.cfg
@@ -15,7 +15,8 @@ path: /home/ste/uploads
 [include config/chamber_2.cfg]
 
 [include config/printhead.cfg]
-[include config/probe.cfg]
+[include config/probe_main.cfg]
+[include config/probe_DAC_v1.cfg]
 [include config/probe_fiber_printer.cfg]
 
 [include config/main_extruder.cfg]

diff --git a/stereotech_config/HTE530-5-3-23.cfg b/stereotech_config/HTE530-5-3-23.cfg
@@ -15,7 +15,8 @@ path: /home/ste/uploads
 [include config/chamber_2.cfg]
 
 [include config/printhead.cfg]
-[include config/probe_2.cfg]
+[include config/probe_main.cfg]
+[include config/probe_DAC_v2.cfg]
 [include config/probe_hybrid_printer.cfg]
 
 [include config/main_extruder.cfg]

diff --git a/stereotech_config/HTE530-5-4-22.cfg b/stereotech_config/HTE530-5-4-22.cfg
@@ -15,7 +15,8 @@ path: /home/ste/uploads
 [include config/chamber_2.cfg]
 
 [include config/printhead.cfg]
-[include config/probe.cfg]
+[include config/probe_main.cfg]
+[include config/probe_DAC_v1.cfg]
 [include config/probe_hybrid_printer.cfg]
 
 [include config/main_extruder.cfg]

diff --git a/stereotech_config/HTE530-5-8-23.cfg b/stereotech_config/HTE530-5-8-23.cfg
@@ -15,7 +15,8 @@ path: /home/ste/uploads
 [include config/chamber_2.cfg]
 
 [include config/printhead.cfg]
-[include config/probe_2.cfg]
+[include config/probe_main.cfg]
+[include config/probe_DAC_v2.cfg]
 [include config/probe_hybrid_printer.cfg]
 
 [include config/main_extruder.cfg]