delta_calibrate: Initial support for enhanced delta calibration

Add support for an enhanced delta calibration routine that performs XY
dimension calibration.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 170 insertions, 17 deletions

diff --git a/klippy/extras/delta_calibrate.py b/klippy/extras/delta_calibrate.py
index 97c75466..0db7436e 100644
--- a/klippy/extras/delta_calibrate.py
+++ b/klippy/extras/delta_calibrate.py
@@ -56,6 +56,15 @@ def get_stable_position(stepper_position, delta_params):
             for sd, ep, sp in zip(
                     dp.stepdists, dp.abs_endstops, stepper_position)]
 
+# Return a stable position from a cartesian coordinate
+def calc_stable_position(coord, delta_params):
+    dp = delta_params
+    steppos = [
+        math.sqrt(a**2 - (t[0]-coord[0])**2 - (t[1]-coord[1])**2) + coord[2]
+        for t, a in zip(dp.towers, dp.arms) ]
+    return [(ep - sp) / sd
+            for sd, ep, sp in zip(dp.stepdists, dp.abs_endstops, steppos)]
+
 # Load a stable position from a config entry
 def load_config_stable(config, option):
     spos = config.get(option)
@@ -69,6 +78,60 @@ def load_config_stable(config, option):
 
 
 ######################################################################
+# Delta calibration object
+######################################################################
+
+# The angles and distances of the calibration object found in
+# docs/prints/calibrate_size.stl
+MeasureAngles = [210., 270., 330., 30., 90., 150.]
+MeasureOuterRadius = 65
+MeasureRidgeRadius = 5. - .5
+
+# How much to prefer a distance measurement over a height measurement
+MEASURE_WEIGHT = 0.5
+
+# Convert distance measurements made on the calibration object to
+# 3-tuples of (actual_distance, stable_position1, stable_position2)
+def measurements_to_distances(measured_params, delta_params):
+    # Extract params
+    mp = measured_params
+    dp = delta_params
+    scale = mp['SCALE'][0]
+    cpw = mp['CENTER_PILLAR_WIDTHS']
+    center_widths = [cpw[0], cpw[2], cpw[1], cpw[0], cpw[2], cpw[1]]
+    center_dists = [od - cw
+                    for od, cw in zip(mp['CENTER_DISTS'], center_widths)]
+    outer_dists = [
+        od - opw
+        for od, opw in zip(mp['OUTER_DISTS'], mp['OUTER_PILLAR_WIDTHS']) ]
+    # Convert angles in degrees to an XY multiplier
+    obj_angles = map(math.radians, MeasureAngles)
+    xy_angles = zip(map(math.cos, obj_angles), map(math.sin, obj_angles))
+    # Calculate stable positions for center measurements
+    inner_ridge = MeasureRidgeRadius * scale
+    inner_pos = [(ax * inner_ridge, ay * inner_ridge, 0.)
+                 for ax, ay in xy_angles]
+    outer_ridge = (MeasureOuterRadius + MeasureRidgeRadius) * scale
+    outer_pos = [(ax * outer_ridge, ay * outer_ridge, 0.)
+                 for ax, ay in xy_angles]
+    center_positions = [
+        (cd, calc_stable_position(ip, dp), calc_stable_position(op, dp))
+        for cd, ip, op in zip(center_dists, inner_pos, outer_pos)]
+    # Calculate positions of outer measurements
+    outer_center = MeasureOuterRadius * scale
+    start_pos = [(ax * outer_center, ay * outer_center) for ax, ay in xy_angles]
+    shifted_angles = xy_angles[2:] + xy_angles[:2]
+    first_pos = [(ax * inner_ridge + spx, ay * inner_ridge + spy, 0.)
+                 for (ax, ay), (spx, spy) in zip(shifted_angles, start_pos)]
+    second_pos = [(ax * outer_ridge + spx, ay * outer_ridge + spy, 0.)
+                  for (ax, ay), (spx, spy) in zip(shifted_angles, start_pos)]
+    outer_positions = [
+        (od, calc_stable_position(fp, dp), calc_stable_position(sp, dp))
+        for od, fp, sp in zip(outer_dists, first_pos, second_pos)]
+    return center_positions + outer_positions
+
+
+######################################################################
 # Delta Calibrate class
 ######################################################################
 
@@ -95,12 +158,23 @@ class DeltaCalibrate:
                 break
             height_pos = load_config_stable(config, "height%d_pos" % (i,))
             self.last_probe_positions.append((height, height_pos))
-        # Register DELTA_CALIBRATE command
+        # Restore distance measurements
+        self.delta_analyze_entry = {'SCALE': (1.,)}
+        self.last_distances = []
+        for i in range(999):
+            dist = config.getfloat("distance%d" % (i,), None)
+            if dist is None:
+                break
+            distance_pos1 = load_config_stable(config, "distance%d_pos1" % (i,))
+            distance_pos2 = load_config_stable(config, "distance%d_pos2" % (i,))
+            self.last_distances.append((dist, distance_pos1, distance_pos2))
+        # Register gcode commands
         self.gcode = self.printer.lookup_object('gcode')
-        self.gcode.register_command(
-            'DELTA_CALIBRATE', self.cmd_DELTA_CALIBRATE,
-            desc=self.cmd_DELTA_CALIBRATE_help)
-    def save_state(self, probe_positions, params):
+        self.gcode.register_command('DELTA_CALIBRATE', self.cmd_DELTA_CALIBRATE,
+                                    desc=self.cmd_DELTA_CALIBRATE_help)
+        self.gcode.register_command('DELTA_ANALYZE', self.cmd_DELTA_ANALYZE,
+                                    desc=self.cmd_DELTA_ANALYZE_help)
+    def save_state(self, probe_positions, distances, params):
         # Save main delta parameters
         configfile = self.printer.lookup_object('configfile')
         configfile.set('printer', 'delta_radius', "%.6f" % (params['radius']))
@@ -118,10 +192,13 @@ class DeltaCalibrate:
             configfile.set(section, "height%d" % (i,), z_offset)
             configfile.set(section, "height%d_pos" % (i,),
                            "%d,%d,%d" % tuple(spos))
-    cmd_DELTA_CALIBRATE_help = "Delta calibration script"
-    def cmd_DELTA_CALIBRATE(self, params):
-        self.gcode.run_script_from_command("G28")
-        self.probe_helper.start_probe()
+        # Save distance measurements
+        for i, (dist, spos1, spos2) in enumerate(distances):
+            configfile.set(section, "distance%d" % (i,), dist)
+            configfile.set(section, "distance%d_pos1" % (i,),
+                           "%.3f,%.3f,%.3f" % tuple(spos1))
+            configfile.set(section, "distance%d_pos2" % (i,),
+                           "%.3f,%.3f,%.3f" % tuple(spos2))
     def get_probed_position(self):
         kin = self.printer.lookup_object('toolhead').get_kinematics()
         return [s.get_commanded_position() for s in kin.get_steppers()]
@@ -133,24 +210,42 @@ class DeltaCalibrate:
         probe_positions = [(z_offset, get_stable_position(p, delta_params))
                            for p in positions]
         # Perform analysis
-        self.calculate_params(probe_positions)
-    def calculate_params(self, probe_positions):
+        self.calculate_params(probe_positions, self.last_distances)
+    def calculate_params(self, probe_positions, distances):
         # Setup for coordinate descent analysis
         kin = self.printer.lookup_object('toolhead').get_kinematics()
         params = kin.get_calibrate_params()
         orig_delta_params = build_delta_params(params)
-        logging.info("Calculating delta_calibrate with: %s\n"
+        logging.info("Calculating delta_calibrate with:\n%s\n%s\n"
                      "Initial delta_calibrate parameters: %s",
-                     probe_positions, params)
+                     probe_positions, distances, params)
         adj_params = ('radius', 'angle_a', 'angle_b',
                       'endstop_a', 'endstop_b', 'endstop_c')
+        z_weight = 1.
+        if distances:
+            adj_params += ('arm_a', 'arm_b', 'arm_c')
+            z_weight = len(distances) / (MEASURE_WEIGHT * len(probe_positions))
         # Perform coordinate descent
+        call_count = [0]
         def delta_errorfunc(params):
+            call_count[0] += 1
+            if not call_count[0] % 1000:
+                self.gcode.respond_info("Working on calibration...")
+                self.printer.get_reactor().pause(0.)
+            # Build new delta_params for params under test
             delta_params = build_delta_params(params)
+            # Calculate z height errors
             total_error = 0.
             for z_offset, stable_pos in probe_positions:
                 x, y, z = get_position_from_stable(stable_pos, delta_params)
                 total_error += (z - z_offset)**2
+            total_error *= z_weight
+            # Calculate distance errors
+            for dist, stable_pos1, stable_pos2 in distances:
+                x1, y1, z1 = get_position_from_stable(stable_pos1, delta_params)
+                x2, y2, z2 = get_position_from_stable(stable_pos2, delta_params)
+                d = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2)
+                total_error += (d - dist)**2
             return total_error
         new_params = mathutil.coordinate_descent(
             adj_params, params, delta_errorfunc)
@@ -162,19 +257,77 @@ class DeltaCalibrate:
                          get_position_from_stable(spos, orig_delta_params)[2],
                          get_position_from_stable(spos, new_delta_params)[2],
                          z_offset)
+        for dist, spos1, spos2 in distances:
+            x1, y1, z1 = get_position_from_stable(spos1, orig_delta_params)
+            x2, y2, z2 = get_position_from_stable(spos2, orig_delta_params)
+            orig_dist = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2)
+            x1, y1, z1 = get_position_from_stable(spos1, new_delta_params)
+            x2, y2, z2 = get_position_from_stable(spos2, new_delta_params)
+            new_dist = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2)
+            logging.info("distance orig: %.6f new: %.6f goal: %.6f",
+                         orig_dist, new_dist, dist)
         self.gcode.respond_info(
-            "stepper_a: position_endstop: %.6f angle: %.6f\n"
-            "stepper_b: position_endstop: %.6f angle: %.6f\n"
-            "stepper_c: position_endstop: %.6f angle: %.6f\n"
+            "stepper_a: position_endstop: %.6f angle: %.6f arm: %.6f\n"
+            "stepper_b: position_endstop: %.6f angle: %.6f arm: %.6f\n"
+            "stepper_c: position_endstop: %.6f angle: %.6f arm: %.6f\n"
             "delta_radius: %.6f\n"
             "The SAVE_CONFIG command will update the printer config file\n"
             "with these parameters and restart the printer." % (
                 new_params['endstop_a'], new_params['angle_a'],
+                new_params['arm_a'],
                 new_params['endstop_b'], new_params['angle_b'],
+                new_params['arm_b'],
                 new_params['endstop_c'], new_params['angle_c'],
+                new_params['arm_c'],
                 new_params['radius']))
         # Store results for SAVE_CONFIG
-        self.save_state(probe_positions, new_params)
+        self.save_state(probe_positions, distances, new_params)
+    cmd_DELTA_CALIBRATE_help = "Delta calibration script"
+    def cmd_DELTA_CALIBRATE(self, params):
+        self.gcode.run_script_from_command("G28")
+        self.probe_helper.start_probe()
+    def do_extended_calibration(self):
+        # Extract distance positions
+        if len(self.delta_analyze_entry) <= 1:
+            distances = self.last_distances
+        elif len(self.delta_analyze_entry) < 5:
+            raise self.gcode.error("Not all measurements provided")
+        else:
+            kin = self.printer.lookup_object('toolhead').get_kinematics()
+            delta_params = build_delta_params(kin.get_calibrate_params())
+            distances = measurements_to_distances(
+                self.delta_analyze_entry, delta_params)
+        if not self.last_probe_positions:
+            raise self.gcode.error(
+                "Must run basic calibration with DELTA_CALIBRATE first")
+        # Perform analysis
+        self.calculate_params(self.last_probe_positions, distances)
+    cmd_DELTA_ANALYZE_help = "Extended delta calibration tool"
+    def cmd_DELTA_ANALYZE(self, params):
+        # Parse distance measurements
+        args = {'CENTER_DISTS': 6, 'CENTER_PILLAR_WIDTHS': 3,
+                'OUTER_DISTS': 6, 'OUTER_PILLAR_WIDTHS': 6, 'SCALE': 1}
+        for name, count in args.items():
+            if name not in params:
+                continue
+            data = self.gcode.get_str(name, params)
+            try:
+                parts = map(float, data.split(','))
+            except:
+                raise self.gcode.error("Unable to parse parameter '%s'" % (
+                    name,))
+            if len(parts) != count:
+                raise self.gcode.error("Parameter '%s' must have %d values" % (
+                    name, count))
+            self.delta_analyze_entry[name] = parts
+            logging.info("DELTA_ANALYZE %s = %s", name, parts)
+        # Perform analysis if requested
+        if 'CALIBRATE' in params:
+            action = self.gcode.get_str('CALIBRATE', params)
+            actions = {'extended': 1}
+            if action not in actions:
+                raise self.gcode.error("Unknown calibrate action")
+            self.do_extended_calibration()
 
 def load_config(config):
     return DeltaCalibrate(config)