delta: Move low-level delta calibration to delta.py

Move the linear delta specific calibration code from
delta_calibrate.py to delta.py.

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

1 files changed, 10 insertions, 70 deletions

diff --git a/klippy/extras/delta_calibrate.py b/klippy/extras/delta_calibrate.py
index 02507465..75a06278 100644
--- a/klippy/extras/delta_calibrate.py
+++ b/klippy/extras/delta_calibrate.py
@@ -6,76 +6,11 @@
 import math, logging, collections
 import probe, mathutil
 
-
-######################################################################
-# Delta "stable position" coordinates
-######################################################################
-
 # A "stable position" is a 3-tuple containing the number of steps
 # taken since hitting the endstop on each delta tower.  Delta
 # calibration uses this coordinate system because it allows a position
 # to be described independent of the software parameters.
 
-class DeltaCalibration:
-    def __init__(self, params):
-        self.params = dict(params)
-        self.radius = params['radius']
-        self.angles = [params['angle_'+a] for a in 'abc']
-        self.arms = [params['arm_'+a] for a in 'abc']
-        self.endstops = [params['endstop_'+a] for a in 'abc']
-        self.stepdists = [params['stepdist_'+a] for a in 'abc']
-        # Calculate the XY cartesian coordinates of the delta towers
-        radian_angles = [math.radians(a) for a in self.angles]
-        self.towers = [(math.cos(a) * self.radius, math.sin(a) * self.radius)
-                       for a in radian_angles]
-        # Calculate the absolute Z height of each tower endstop
-        radius2 = self.radius**2
-        self.abs_endstops = [e + math.sqrt(a**2 - radius2)
-                             for e, a in zip(self.endstops, self.arms)]
-    def get_position_from_stable(self, stable_position):
-        # Return cartesian coordinates for the given stable_position
-        sphere_coords = [
-            (t[0], t[1], es - sp * sd)
-            for sd, t, es, sp in zip(self.stepdists, self.towers,
-                                     self.abs_endstops, stable_position) ]
-        return mathutil.trilateration(sphere_coords, [a**2 for a in self.arms])
-    def calc_stable_position(self, coord):
-        # Return a stable_position from a cartesian coordinate
-        steppos = [
-            math.sqrt(a**2 - (t[0]-coord[0])**2 - (t[1]-coord[1])**2) + coord[2]
-            for t, a in zip(self.towers, self.arms) ]
-        return [(ep - sp) / sd
-                for sd, ep, sp in zip(self.stepdists,
-                                      self.abs_endstops, steppos)]
-    def coordinate_descent_params(self, is_extended):
-        adj_params = ('radius', 'angle_a', 'angle_b',
-                      'endstop_a', 'endstop_b', 'endstop_c')
-        if is_extended:
-            adj_params += ('arm_a', 'arm_b', 'arm_c')
-        return adj_params, self.params
-    def new_calibration(self, params):
-        return DeltaCalibration(params)
-    def save_state(self, configfile):
-        params = self.params
-        configfile.set('printer', 'delta_radius', "%.6f" % (params['radius']))
-        for axis in 'abc':
-            configfile.set('stepper_'+axis, 'angle',
-                           "%.6f" % (params['angle_'+axis],))
-            configfile.set('stepper_'+axis, 'arm_length',
-                           "%.6f" % (params['arm_'+axis],))
-            configfile.set('stepper_'+axis, 'position_endstop',
-                           "%.6f" % (params['endstop_'+axis],))
-        gcode = configfile.get_printer().lookup_object("gcode")
-        gcode.respond_info(
-            "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"
-            % (params['endstop_a'], params['angle_a'], params['arm_a'],
-               params['endstop_b'], params['angle_b'], params['arm_b'],
-               params['endstop_c'], params['angle_c'], params['arm_c'],
-               params['radius']))
-
 # Load a stable position from a config entry
 def load_config_stable(config, option):
     spos = config.get(option)
@@ -149,8 +84,8 @@ def measurements_to_distances(measured_params, delta_params):
 class DeltaCalibrate:
     def __init__(self, config):
         self.printer = config.get_printer()
-        if config.getsection('printer').get('kinematics') != 'delta':
-            raise config.error("Delta calibrate is only for delta printers")
+        self.printer.register_event_handler("klippy:connect",
+                                            self.handle_connect)
         # Calculate default probing points
         radius = config.getfloat('radius', above=0.)
         points = [(0., 0.)]
@@ -186,6 +121,11 @@ class DeltaCalibrate:
                                     desc=self.cmd_DELTA_CALIBRATE_help)
         self.gcode.register_command('DELTA_ANALYZE', self.cmd_DELTA_ANALYZE,
                                     desc=self.cmd_DELTA_ANALYZE_help)
+    def handle_connect(self):
+        kin = self.printer.lookup_object('toolhead').get_kinematics()
+        if not hasattr(kin, "get_calibration"):
+            raise self.printer.config_error(
+                "Delta calibrate is only for delta printers")
     def save_state(self, probe_positions, distances, delta_params):
         # Save main delta parameters
         configfile = self.printer.lookup_object('configfile')
@@ -208,7 +148,7 @@ class DeltaCalibrate:
         # Convert positions into (z_offset, stable_position) pairs
         z_offset = offsets[2]
         kin = self.printer.lookup_object('toolhead').get_kinematics()
-        delta_params = DeltaCalibration(kin.get_calibrate_params())
+        delta_params = kin.get_calibration()
         probe_positions = [(z_offset, delta_params.calc_stable_position(p))
                            for p in positions]
         # Perform analysis
@@ -216,7 +156,7 @@ class DeltaCalibrate:
     def calculate_params(self, probe_positions, distances):
         # Setup for coordinate descent analysis
         kin = self.printer.lookup_object('toolhead').get_kinematics()
-        orig_delta_params = odp = DeltaCalibration(kin.get_calibrate_params())
+        orig_delta_params = odp = kin.get_calibration()
         adj_params, params = odp.coordinate_descent_params(distances)
         logging.info("Calculating delta_calibrate with:\n%s\n%s\n"
                      "Initial delta_calibrate parameters: %s",
@@ -276,7 +216,7 @@ class DeltaCalibrate:
             raise self.gcode.error("Not all measurements provided")
         else:
             kin = self.printer.lookup_object('toolhead').get_kinematics()
-            delta_params = DeltaCalibration(kin.get_calibrate_params())
+            delta_params = kin.get_calibration()
             distances = measurements_to_distances(
                 self.delta_analyze_entry, delta_params)
         if not self.last_probe_positions: