bed_mesh: mesh bed leveling for Klipper

Initial implementation of configurable Mesh Bed Leveling.

Signed-off-by: Eric Callahan <arksine.code@gmail.com>

1 files changed, 586 insertions, 0 deletions

diff --git a/klippy/extras/bed_mesh.py b/klippy/extras/bed_mesh.py
new file mode 100644
index 00000000..4c006ea2
--- /dev/null
+++ b/klippy/extras/bed_mesh.py
@@ -0,0 +1,586 @@
+# Mesh Bed Leveling
+#
+# Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2018 Eric Callahan <arksine.code@gmail.com>
+#
+# This file may be distributed under the terms of the GNU GPLv3 license.
+import logging
+import math
+import json
+import probe
+
+class BedMeshError(Exception):
+    pass
+
+# PEP 485 isclose()
+def isclose(a, b, rel_tol=1e-09, abs_tol=0.0):
+    return abs(a-b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol)
+
+# Constrain value between min and max
+def constrain(val, min_val, max_val):
+    return min(max_val, max(min_val, val))
+
+# Linear interpolation between two values
+def lerp(t, v0, v1):
+    return (1. - t) * v0 + t * v1
+
+# retreive commma separated pair from config
+def parse_pair(config, param, check=True, cast=float,
+               minval=None, maxval=None):
+    val = config.get(*param).strip().split(',', 1)
+    pair = tuple(cast(p.strip()) for p in val)
+    if check and len(pair) != 2:
+        raise config.error(
+            "bed_mesh: malformed '%s' value: %s"
+            % (param[0], config.get(*param)))
+    elif len(pair) == 1:
+        pair = (pair[0], pair[0])
+    if minval is not None:
+        if pair[0] < minval or pair[1] < minval:
+            raise config.error(
+                "Option '%s' in section bed_mesh must have a minimum of %s"
+                % (param[0]), minval)
+    if maxval is not None:
+        if pair[0] > maxval or pair[1] > maxval:
+            raise config.error(
+                "Option '%s' in section bed_mesh must have a maximum of %s"
+                % (param[0]), str(minval))
+    return pair
+
+
+class BedMesh:
+    FADE_DISABLE = 0x7FFFFFFF
+    def __init__(self, config):
+        self.printer = config.get_printer()
+        self.last_position = [0., 0., 0., 0.]
+        self.calibrate = BedMeshCalibrate(config, self)
+        self.z_mesh = None
+        self.toolhead = None
+        self.horizontal_move_z = config.getfloat('horizontal_move_z', 5.)
+        self.fade_start = config.getfloat('fade_start', 1.)
+        self.fade_end = config.getfloat('fade_end', 10.)
+        self.fade_dist = self.fade_end - self.fade_start
+        if self.fade_dist <= 0.:
+            self.fade_start = self.fade_end = self.FADE_DISABLE
+        self.gcode = self.printer.lookup_object('gcode')
+        self.splitter = MoveSplitter(config, self.gcode)
+        self.gcode.register_command(
+            'BED_MESH_OUTPUT', self.cmd_BED_MESH_OUTPUT,
+            desc=self.cmd_BED_MESH_OUTPUT_help)
+        self.gcode.register_command(
+            'BED_MESH_CLEAR', self.cmd_BED_MESH_CLEAR,
+            desc=self.cmd_BED_MESH_CLEAR_help)
+        self.gcode.set_move_transform(self)
+    def printer_state(self, state):
+        if state == 'connect':
+            self.toolhead = self.printer.lookup_object('toolhead')
+    def set_mesh(self, mesh):
+        # Assign the current mesh.  If set to None, no transform
+        # is applied
+        self.z_mesh = mesh
+        self.splitter.set_mesh(mesh)
+        # cache the current position before a transform takes place
+        self.last_position[:] = self.toolhead.get_position()
+    def get_z_factor(self, z_pos):
+        if z_pos >= self.fade_end:
+            return 0.
+        elif z_pos >= self.fade_start:
+            return (self.fade_end - z_pos) / self.fade_dist
+        else:
+            return 1.
+    def get_position(self):
+        # Return last, non-transformed position
+        if self.z_mesh is None:
+            # No mesh calibrated, so send toolhead position
+            return self.toolhead.get_position()
+        else:
+            # return current position minus the current z-adjustment
+            x, y, z, e = self.toolhead.get_position()
+            z_adjust = self.get_z_factor(z) * self.z_mesh.get_z(x, y)
+            return [x, y, z - z_adjust, e]
+    def move(self, newpos, speed):
+        factor = self.get_z_factor(newpos[2])
+        if self.z_mesh is None or not factor:
+            # No mesh calibrated, or mesh leveling phased out.
+            self.toolhead.move(newpos, speed)
+        else:
+            self.splitter.build_move(self.last_position, newpos, factor)
+            while not self.splitter.traverse_complete:
+                split_move = self.splitter.split()
+                if split_move:
+                    self.toolhead.move(split_move, speed)
+                else:
+                    raise self.gcode.error(
+                        "Mesh Leveling: Error splitting move ")
+        self.last_position[:] = newpos
+    cmd_BED_MESH_OUTPUT_help = "Retrieve interpolated grid of probed z-points"
+    def cmd_BED_MESH_OUTPUT(self, params):
+        if self.z_mesh is None:
+            self.gcode.respond_info("Bed has not been probed")
+        else:
+            self.calibrate.print_probed_positions(self.gcode.respond_info)
+            self.z_mesh.print_mesh(self.gcode.respond, self.horizontal_move_z)
+    cmd_BED_MESH_CLEAR_help = "Clear the Mesh so no z-adjusment is made"
+    def cmd_BED_MESH_CLEAR(self, params):
+        self.set_mesh(None)
+
+
+class BedMeshCalibrate:
+    ALGOS = ['lagrange', 'bicubic']
+    def __init__(self, config, bedmesh):
+        self.printer = config.get_printer()
+        self.bedmesh = bedmesh
+        self.probed_z_table = None
+        self.build_map = False
+        self.probe_params = {}
+        points = self._generate_points(config)
+        self._init_probe_params(config, points)
+        self.probe_helper = probe.ProbePointsHelper(config, self, points)
+        self.z_offset_check = None
+        if config.has_section('stepper_z'):
+            zconfig = config.getsection('stepper_z')
+            self.z_offset_check = zconfig.getfloat('position_endstop', None)
+        self.gcode = self.printer.lookup_object('gcode')
+        self.gcode.register_command(
+            'BED_MESH_CALIBRATE', self.cmd_BED_MESH_CALIBRATE,
+            desc=self.cmd_BED_MESH_CALIBRATE_help)
+        self.gcode.register_command(
+            'BED_MESH_MAP', self.cmd_BED_MESH_MAP,
+            desc=self.cmd_BED_MESH_MAP_help)
+    def _generate_points(self, config):
+        x_cnt, y_cnt = parse_pair(
+            config, ('probe_count', '3'), check=False, cast=int, minval=3)
+        self.probe_params['x_count'] = x_cnt
+        self.probe_params['y_count'] = y_cnt
+        min_x, min_y = parse_pair(config, ('min_point',))
+        max_x, max_y = parse_pair(config, ('max_point',))
+        if max_x <= min_x or max_y <= min_y:
+            raise config.error('bed_mesh: invalid min/max points')
+        x_dist = (max_x - min_x) / (x_cnt - 1)
+        y_dist = (max_y - min_y) / (y_cnt - 1)
+        # floor distances down to next hundredth
+        x_dist = math.floor(x_dist * 100) / 100
+        y_dist = math.floor(y_dist * 100) / 100
+        if x_dist <= 1. or y_dist <= 1.:
+            raise config.error("bed_mesh: min/max points too close together")
+        # re-calc x_max
+        max_x = min_x + x_dist * (x_cnt - 1)
+        pos_y = min_y
+        points = []
+        for i in range(y_cnt):
+            for j in range(x_cnt):
+                if not i % 2:
+                    # move in positive directon
+                    pos_x = min_x + j * x_dist
+                else:
+                    # move in negative direction
+                    pos_x = max_x - j * x_dist
+                points.append((pos_x, pos_y))
+            pos_y += y_dist
+        logging.info('bed_mesh: generated points')
+        for p in points:
+            logging.info("(%.1f, %.1f)" % (p[0], p[1]))
+        return points
+    def _init_probe_params(self, config, points):
+        self.probe_params['min_x'] = min(points, key=lambda p: p[0])[0]
+        self.probe_params['max_x'] = max(points, key=lambda p: p[0])[0]
+        self.probe_params['min_y'] = min(points, key=lambda p: p[1])[1]
+        self.probe_params['max_y'] = max(points, key=lambda p: p[1])[1]
+        offset = parse_pair(config, ('probe_offset',))
+        self.probe_params['x_offset'] = offset[0]
+        self.probe_params['y_offset'] = offset[1]
+        pps = parse_pair(config, ('mesh_pps', '2'), check=False,
+                         cast=int, minval=0)
+        self.probe_params['mesh_x_pps'] = pps[0]
+        self.probe_params['mesh_y_pps'] = pps[1]
+        self.probe_params['algo'] = config.get('algorithm', 'lagrange') \
+                                          .strip().lower()
+        if self.probe_params['algo'] not in self.ALGOS:
+            raise config.error(
+                "bed_mesh: Unknown algorithm <%s>"
+                % (self.probe_params['algo']))
+        self.probe_params['tension'] = config.getfloat(
+            'bicubic_tension', .2, minval=0., maxval=2.)
+        logging.debug('bed_mesh: probe/mesh parameters:')
+        for key, value in self.probe_params.iteritems():
+            logging.debug("%s :  %s" % (key, value))
+    cmd_BED_MESH_MAP_help = "Probe the bed and serialize output"
+    def cmd_BED_MESH_MAP(self, params):
+        self.build_map = True
+        self.start_calibration()
+    cmd_BED_MESH_CALIBRATE_help = "Perform Mesh Bed Leveling"
+    def cmd_BED_MESH_CALIBRATE(self, params):
+        self.build_map = False
+        self.start_calibration()
+    def start_calibration(self):
+        self.bedmesh.set_mesh(None)
+        self.gcode.run_script_from_command("G28")
+        self.probe_helper.start_probe()
+    def get_probed_position(self):
+        kin = self.printer.lookup_object('toolhead').get_kinematics()
+        return kin.calc_position()
+    def print_probed_positions(self, print_func):
+        if self.probed_z_table is not None:
+            msg = "Mesh Leveling Probed Z positions:\n"
+            for line in self.probed_z_table:
+                for x in line:
+                    msg += " %f" % x
+                msg += "\n"
+            print_func(msg)
+        else:
+            print_func("bed_mesh: bed has not been probed")
+    def finalize(self, z_offset, positions):
+        if self.z_offset_check is not None:
+            if self.z_offset_check != z_offset:
+                z_msg = "bed_mesh: WARN - probe z_offset is not" \
+                        " equal to Z position_endstop\n"
+                z_msg += "[probe] z_offset: %.4f\n" % z_offset
+                z_msg += "[stepper_z] position_endstop: %.4f" \
+                    % self.z_offset_check
+                logging.info(z_msg)
+                self.gcode.respond_info(z_msg)
+        x_cnt = self.probe_params['x_count']
+        y_cnt = self.probe_params['y_count']
+        # create a 2-D array representing the probed z-positions.
+        self.probed_z_table = [
+            [0. for i in range(x_cnt)] for j in range(y_cnt)]
+        # Extract probed z-positions from probed positions and add
+        # them to organized list
+        for i, pos in enumerate(positions):
+            y_position = i / x_cnt
+            x_position = 0
+            if y_position & 1 == 0:
+                # Even y count, x probed in positive directon
+                x_position = i % x_cnt
+            else:
+                # Odd y count, x probed in the negative directon
+                x_position = (x_cnt - 1) - (i % x_cnt)
+            self.probed_z_table[y_position][x_position] = \
+                pos[2] - z_offset
+        if self.build_map:
+            outdict = {'z_probe_offsets:': self.probed_z_table}
+            self.gcode.respond(json.dumps(outdict))
+        else:
+            mesh = ZMesh(self.probe_params)
+            try:
+                mesh.build_mesh(self.probed_z_table)
+            except BedMeshError as e:
+                raise self.gcode.error(e.message)
+            self.bedmesh.set_mesh(mesh)
+            self.print_probed_positions(logging.debug)
+            self.gcode.respond_info("Mesh Bed Leveling Complete")
+
+
+class MoveSplitter:
+    def __init__(self, config, gcode):
+        self.split_delta_z = config.getfloat(
+            'split_delta_z', .025, minval=0.01)
+        self.move_check_distance = config.getfloat(
+            'move_check_distance', 5., minval=3.)
+        self.z_mesh = None
+        self.gcode = gcode
+    def set_mesh(self, mesh):
+        self.z_mesh = mesh
+    def build_move(self, prev_pos, next_pos, factor):
+        self.prev_pos = tuple(prev_pos)
+        self.next_pos = tuple(next_pos)
+        self.current_pos = list(prev_pos)
+        self.z_factor = factor
+        self.z_offset =  \
+            self.z_factor \
+            * self.z_mesh.get_z(self.prev_pos[0], self.prev_pos[1])
+        self.traverse_complete = False
+        self.distance_checked = 0.
+        axes_d = [self.next_pos[i] - self.prev_pos[i] for i in range(4)]
+        self.total_move_length = math.sqrt(sum([d*d for d in axes_d[:3]]))
+        self.axis_move = [not isclose(d, 0., abs_tol=1e-10) for d in axes_d]
+    def _set_next_move(self, distance_from_prev):
+        t = distance_from_prev / self.total_move_length
+        if t > 1. or t < 0.:
+            raise self.gcode.error(
+                "bed_mesh: Slice distance is negative "
+                "or greater than entire move length")
+        for i in range(4):
+            if self.axis_move[i]:
+                self.current_pos[i] = lerp(
+                    t, self.prev_pos[i], self.next_pos[i])
+    def split(self):
+        if not self.traverse_complete:
+            if self.axis_move[0] or self.axis_move[1]:
+                # X and/or Y axis move, traverse if necessary
+                while self.distance_checked + self.move_check_distance \
+                        < self.total_move_length:
+                    self.distance_checked += self.move_check_distance
+                    self._set_next_move(self.distance_checked)
+                    next_z = \
+                        self.z_factor \
+                        * self.z_mesh.get_z(
+                            self.current_pos[0], self.current_pos[1])
+                    if abs(next_z - self.z_offset) >= self.split_delta_z:
+                        self.z_offset = next_z
+                        return self.current_pos[0], self.current_pos[1], \
+                            self.current_pos[2] + self.z_offset, \
+                            self.current_pos[3]
+            # end of move reached
+            self.current_pos[:] = self.next_pos
+            self.z_offset = \
+                self.z_factor \
+                * self.z_mesh.get_z(self.current_pos[0], self.current_pos[1])
+            # Its okay to add Z-Offset to the final move, since it will not be
+            # used again.
+            self.current_pos[2] += self.z_offset
+            self.traverse_complete = True
+            return self.current_pos
+        else:
+            # Traverse complete
+            return None
+
+
+class ZMesh:
+    def __init__(self, params):
+        self.mesh_z_table = None
+        self.probe_params = params
+        self.mesh_x_min = params['min_x'] + params['x_offset']
+        self.mesh_x_max = params['max_x'] + params['x_offset']
+        self.mesh_y_min = params['min_y'] + params['y_offset']
+        self.mesh_y_max = params['max_y'] + params['y_offset']
+        logging.debug(
+            "bed_mesh: Mesh Min: (%.2f,%.2f) Mesh Max: (%.2f,%.2f)"
+            % (self.mesh_x_min, self.mesh_y_min,
+               self.mesh_x_max, self.mesh_y_max))
+        if params['algo'] == 'bicubic':
+            self.build_mesh = self._sample_bicubic
+        else:
+            self.build_mesh = self._sample_lagrange
+        # Nummber of points to interpolate per segment
+        mesh_x_pps = params['mesh_x_pps']
+        mesh_y_pps = params['mesh_y_pps']
+        px_cnt = params['x_count']
+        py_cnt = params['y_count']
+        mesh_x_mult = mesh_x_pps + 1
+        mesh_y_mult = mesh_y_pps + 1
+        if px_cnt == 3 or py_cnt == 3:
+            # a mesh with 3 points on either axis defaults to legrange
+            # upsampling
+            self.build_mesh = self._sample_lagrange
+            self.probe_params['algo'] = 'lagrange'
+        if mesh_x_mult == 1 and mesh_y_mult == 1:
+            # No interpolation, sample the probed points directly
+            self.build_mesh = self._sample_direct
+            self.probe_params['algo'] = 'direct'
+        self.mesh_x_count = px_cnt * mesh_x_mult - (mesh_x_mult - 1)
+        self.mesh_y_count = py_cnt * mesh_y_mult - (mesh_y_mult - 1)
+        self.x_mult = mesh_x_mult
+        self.y_mult = mesh_y_mult
+        logging.debug("bed_mesh: Mesh grid size - X:%d, Y:%d"
+                      % (self.mesh_x_count, self.mesh_y_count))
+        self.mesh_x_dist = (self.mesh_x_max - self.mesh_x_min) / \
+                           (self.mesh_x_count - 1)
+        self.mesh_y_dist = (self.mesh_y_max - self.mesh_y_min) / \
+                           (self.mesh_y_count - 1)
+    def get_x_coordinate(self, index):
+        return self.mesh_x_min + self.mesh_x_dist * index
+    def get_y_coordinate(self, index):
+        return self.mesh_y_min + self.mesh_y_dist * index
+    def get_z(self, x, y):
+        if self.mesh_z_table:
+            tbl = self.mesh_z_table
+            tx, xidx = self._get_linear_index(x, 0)
+            ty, yidx = self._get_linear_index(y, 1)
+            z0 = lerp(tx, tbl[yidx][xidx], tbl[yidx][xidx+1])
+            z1 = lerp(tx, tbl[yidx+1][xidx], tbl[yidx+1][xidx+1])
+            return lerp(ty, z0, z1)
+        else:
+            # No mesh table generated, no z-adjustment
+            return 0.
+    def print_mesh(self, print_func, move_z=None):
+        if self.mesh_z_table is not None:
+            msg = "Mesh X,Y: %d,%d\n" % (self.mesh_x_count, self.mesh_y_count)
+            if move_z is not None:
+                msg += "Search Height: %d\n" % (move_z)
+            msg += "Interpolation Algorithm: %s\n" \
+                   % (self.probe_params['algo'])
+            msg += "Measured points:\n"
+            for y_line in range(self.mesh_y_count - 1, -1, -1):
+                for z in self.mesh_z_table[y_line]:
+                    msg += "  %f" % (z)
+                msg += "\n"
+            print_func(msg)
+        else:
+            print_func("bed_mesh: Z Mesh not generated")
+    def _get_linear_index(self, coord, axis):
+        if axis == 0:
+            # X-axis
+            mesh_min = self.mesh_x_min
+            mesh_cnt = self.mesh_x_count
+            mesh_dist = self.mesh_x_dist
+            cfunc = self.get_x_coordinate
+        else:
+            # Y-axis
+            mesh_min = self.mesh_y_min
+            mesh_cnt = self.mesh_y_count
+            mesh_dist = self.mesh_y_dist
+            cfunc = self.get_y_coordinate
+        t = 0.
+        idx = int(math.floor((coord - mesh_min) / mesh_dist))
+        idx = constrain(idx, 0, mesh_cnt - 2)
+        t = (coord - cfunc(idx)) / mesh_dist
+        return constrain(t, 0., 1.), idx
+    def _sample_direct(self, z_table):
+        self.mesh_z_table = z_table
+    def _sample_lagrange(self, z_table):
+        x_mult = self.x_mult
+        y_mult = self.y_mult
+        self.mesh_z_table = \
+            [[0. if ((i % x_mult) or (j % y_mult))
+             else z_table[j/y_mult][i/x_mult]
+             for i in range(self.mesh_x_count)]
+             for j in range(self.mesh_y_count)]
+        xpts, ypts = self._get_lagrange_coords(z_table)
+        # Interpolate X coordinates
+        for i in range(self.mesh_y_count):
+            # only interpolate X-rows that have probed coordinates
+            if i % y_mult != 0:
+                continue
+            for j in range(self.mesh_x_count):
+                if j % x_mult == 0:
+                    continue
+                x = self.get_x_coordinate(j)
+                self.mesh_z_table[i][j] = self._calc_lagrange(xpts, x, i, 0)
+        # Interpolate Y coordinates
+        for i in range(self.mesh_x_count):
+            for j in range(self.mesh_y_count):
+                if j % y_mult == 0:
+                    continue
+                y = self.get_y_coordinate(j)
+                self.mesh_z_table[j][i] = self._calc_lagrange(ypts, y, i, 1)
+        self.print_mesh(logging.debug)
+    def _get_lagrange_coords(self, z_table):
+        xpts = []
+        ypts = []
+        for i in range(self.probe_params['x_count']):
+            xpts.append(self.get_x_coordinate(i * self.x_mult))
+        for j in range(self.probe_params['y_count']):
+            ypts.append(self.get_y_coordinate(j * self.y_mult))
+        return xpts, ypts
+    def _calc_lagrange(self, lpts, c, vec, axis=0):
+        pt_cnt = len(lpts)
+        total = 0.
+        for i in range(pt_cnt):
+            n = 1.
+            d = 1.
+            for j in range(pt_cnt):
+                if j == i:
+                    continue
+                n *= (c - lpts[j])
+                d *= (lpts[i] - lpts[j])
+            if axis == 0:
+                # Calc X-Axis
+                z = self.mesh_z_table[vec][i*self.x_mult]
+            else:
+                # Calc Y-Axis
+                z = self.mesh_z_table[i*self.y_mult][vec]
+            total += z * n / d
+        return total
+    def _sample_bicubic(self, z_table):
+        # should work for any number of probe points above 3x3
+        x_mult = self.x_mult
+        y_mult = self.y_mult
+        c = self.probe_params['tension']
+        self.mesh_z_table = \
+            [[0. if ((i % x_mult) or (j % y_mult))
+             else z_table[j/y_mult][i/x_mult]
+             for i in range(self.mesh_x_count)]
+             for j in range(self.mesh_y_count)]
+        # Interpolate X values
+        for y in range(self.mesh_y_count):
+            if y % y_mult != 0:
+                continue
+            for x in range(self.mesh_x_count):
+                if x % x_mult == 0:
+                    continue
+                pts = self._get_x_ctl_pts(x, y)
+                self.mesh_z_table[y][x] = self._cardinal_spline(pts, c)
+        # Interpolate Y values
+        for x in range(self.mesh_x_count):
+            for y in range(self.mesh_y_count):
+                if y % y_mult == 0:
+                    continue
+                pts = self._get_y_ctl_pts(x, y)
+                self.mesh_z_table[y][x] = self._cardinal_spline(pts, c)
+        self.print_mesh(logging.debug)
+    def _get_x_ctl_pts(self, x, y):
+        # Fetch control points and t for a X value in the mesh
+        x_mult = self.x_mult
+        x_row = self.mesh_z_table[y]
+        last_pt = self.mesh_x_count - 1 - x_mult
+        if x < x_mult:
+            p0 = p1 = x_row[0]
+            p2 = x_row[x_mult]
+            p3 = x_row[2*x_mult]
+            t = x / float(x_mult)
+        elif x > last_pt:
+            p0 = x_row[last_pt - x_mult]
+            p1 = x_row[last_pt]
+            p2 = p3 = x_row[last_pt + x_mult]
+            t = (x - last_pt) / float(x_mult)
+        else:
+            found = False
+            for i in range(x_mult, last_pt, x_mult):
+                if x > i and x < (i + x_mult):
+                    p0 = x_row[i - x_mult]
+                    p1 = x_row[i]
+                    p2 = x_row[i + x_mult]
+                    p3 = x_row[i + 2*x_mult]
+                    t = (x - i) / float(x_mult)
+                    found = True
+                    break
+            if not found:
+                raise BedMeshError(
+                    "bed_mesh: Error finding x control points")
+        return p0, p1, p2, p3, t
+    def _get_y_ctl_pts(self, x, y):
+        # Fetch control points and t for a Y value in the mesh
+        y_mult = self.y_mult
+        last_pt = self.mesh_y_count - 1 - y_mult
+        y_col = self.mesh_z_table
+        if y < y_mult:
+            p0 = p1 = y_col[0][x]
+            p2 = y_col[y_mult][x]
+            p3 = y_col[2*y_mult][x]
+            t = y / float(y_mult)
+        elif y > last_pt:
+            p0 = y_col[last_pt - y_mult][x]
+            p1 = y_col[last_pt][x]
+            p2 = p3 = y_col[last_pt + y_mult][x]
+            t = (y - last_pt) / float(y_mult)
+        else:
+            found = False
+            for i in range(y_mult, last_pt, y_mult):
+                if y > i and y < (i + y_mult):
+                    p0 = y_col[i - y_mult][x]
+                    p1 = y_col[i][x]
+                    p2 = y_col[i + y_mult][x]
+                    p3 = y_col[i + 2*y_mult][x]
+                    t = (y - i) / float(y_mult)
+                    found = True
+                    break
+            if not found:
+                raise BedMeshError(
+                    "bed_mesh: Error finding y control points")
+        return p0, p1, p2, p3, t
+    def _cardinal_spline(self, p, tension):
+        t = p[4]
+        t2 = t*t
+        t3 = t2*t
+        m1 = tension * (p[2] - p[0])
+        m2 = tension * (p[3] - p[1])
+        a = p[1] * (2*t3 - 3*t2 + 1)
+        b = p[2] * (-2*t3 + 3*t2)
+        c = m1 * (t3 - 2*t2 + t)
+        d = m2 * (t3 - t2)
+        return a + b + c + d
+
+
+def load_config(config):
+    return BedMesh(config)