feat: Flex compensation for Hangprinters

config/example-winch.cfg

@@ -83,3 +83,9 @@ serial: /dev/ttyACM0
 kinematics: winch
 max_velocity: 300
 max_accel: 3000
+winch_mover_weight: 0.0
+winch_spring_constant: 0.0
+winch_target_force: 0.0
+winch_min_force: 0.0, 0.0, 0.0, 0.0
+winch_max_force: 120.0, 120.0, 120.0, 120.0
+# winch_guy_wire_lengths: 0.0, 0.0, 0.0, 0.0

docs/Config_Reference.md

@@ -695,6 +695,27 @@ movement commands until the toolhead is at 0, 0, 0 and then issue a
 ```
 [printer]
 kinematics: winch
+winch_mover_weight:
+#   The mover weight in kilograms. Set to 0 to disable flex
+#   compensation. The default is 0 (disabled).
+winch_spring_constant:
+#   Approximate spring constant of the line in Newton per meter. The
+#   default is 0.
+winch_target_force:
+#   Desired pre-tension per low anchor in Newton. It is common to set
+#   this to roughly ten times the mover weight. The default is 0.
+winch_min_force:
+#   Minimum permitted force per anchor in Newton. Provide one value
+#   per defined anchor (comma separated). The default is zero for each
+#   anchor.
+winch_max_force:
+#   Maximum permitted force per anchor in Newton. Provide one value
+#   per defined anchor (comma separated). The default is 120 for each
+#   anchor.
+#winch_guy_wire_lengths:
+#   Optional guy wire lengths in millimeters for each anchor. If
+#   omitted, the firmware assumes all low anchors route through the
+#   top anchor.
 
 # The stepper_a section describes the stepper connected to the first
 # cable winch. A minimum of 3 and a maximum of 26 cable winches may be

klippy/chelper/__init__.py

@@ -151,8 +151,17 @@ defs_kin_rotary_delta = """
 """
 
 defs_kin_winch = """
-    struct stepper_kinematics *winch_stepper_alloc(double anchor_x
-        , double anchor_y, double anchor_z);
+    struct winch_flex *winch_flex_alloc(void);
+    void winch_flex_free(struct winch_flex *wf);
+    void winch_flex_configure(struct winch_flex *wf, int num_anchors,
+        const double *anchors, double mover_weight, double spring_constant,
+        double target_force, const double *min_force,
+        const double *max_force, const double *guy_wires,
+        int guy_wires_valid);
+    void winch_flex_calc_arrays(struct winch_flex *wf, double x, double y,
+        double z, double *distances_out, double *flex_out);
+    struct stepper_kinematics *winch_stepper_alloc(struct winch_flex *wf,
+        int index);
 """
 
 defs_kin_extruder = """

klippy/chelper/kin_winch.c

@@ -1,10 +1,11 @@
-// Cable winch stepper kinematics
+// Cable winch stepper kinematics with flex compensation
 //
 // Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2024       Contributors
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
-#include <math.h> // sqrt
+#include <math.h>   // sqrt, fabs, fmax, fmin
 #include <stddef.h> // offsetof
 #include <stdlib.h> // malloc
 #include <string.h> // memset
@@ -12,31 +13,488 @@
 #include "itersolve.h" // struct stepper_kinematics
 #include "trapq.h" // move_get_coord
 
-struct winch_stepper {
-    struct stepper_kinematics sk;
-    struct coord anchor;
+#define WINCH_MAX_ANCHORS 26
+#define EPSILON 1e-9
+#define G_ACCEL 9.81
+
+struct winch_flex {
+    int num_anchors;
+    int flex_enabled;
+    int guy_wires_explicit;
+    struct coord anchors[WINCH_MAX_ANCHORS];
+    double mover_weight;
+    double spring_constant;
+    double target_force;
+    double min_force[WINCH_MAX_ANCHORS];
+    double max_force[WINCH_MAX_ANCHORS];
+    double guy_wires[WINCH_MAX_ANCHORS];
+    double distances_origin[WINCH_MAX_ANCHORS];
+    double relaxed_origin[WINCH_MAX_ANCHORS];
+    double cache_distances[WINCH_MAX_ANCHORS];
+    double cache_flex[WINCH_MAX_ANCHORS];
+    struct move *cache_move;
+    double cache_time;
+    int cache_valid;
 };
 
-static double
-winch_stepper_calc_position(struct stepper_kinematics *sk, struct move *m
-                            , double move_time)
+struct winch_stepper {
+    struct stepper_kinematics sk;
+    struct winch_flex *wf;
+    int index;
+};
+
+static inline double
+clampd(double v, double min_v, double max_v)
+{
+    if (max_v > min_v) {
+        if (v < min_v)
+            return min_v;
+        if (v > max_v)
+            return max_v;
+    }
+    return v;
+}
+
+static inline double
+safe_ratio(double num, double den)
+{
+    double abs_den = fabs(den);
+    if (abs_den < EPSILON)
+        return (num >= 0.) ? 1e9 : -1e9;
+    return num / den;
+}
+
+static inline double
+hypot3(double dx, double dy, double dz)
 {
-    struct winch_stepper *hs = container_of(sk, struct winch_stepper, sk);
-    struct coord c = move_get_coord(m, move_time);
-    double dx = hs->anchor.x - c.x, dy = hs->anchor.y - c.y;
-    double dz = hs->anchor.z - c.z;
     return sqrt(dx*dx + dy*dy + dz*dz);
 }
 
-struct stepper_kinematics * __visible
-winch_stepper_alloc(double anchor_x, double anchor_y, double anchor_z)
+static int
+gauss_jordan_3x5(double M[3][5])
 {
-    struct winch_stepper *hs = malloc(sizeof(*hs));
-    memset(hs, 0, sizeof(*hs));
-    hs->anchor.x = anchor_x;
-    hs->anchor.y = anchor_y;
-    hs->anchor.z = anchor_z;
-    hs->sk.calc_position_cb = winch_stepper_calc_position;
-    hs->sk.active_flags = AF_X | AF_Y | AF_Z;
-    return &hs->sk;
+    for (int col = 0; col < 3; ++col) {
+        int pivot = col;
+        double max_val = fabs(M[pivot][col]);
+        for (int row = col + 1; row < 3; ++row) {
+            double val = fabs(M[row][col]);
+            if (val > max_val) {
+                max_val = val;
+                pivot = row;
+            }
+        }
+        if (max_val < EPSILON)
+            return 0;
+        if (pivot != col) {
+            for (int c = col; c < 5; ++c) {
+                double tmp = M[col][c];
+                M[col][c] = M[pivot][c];
+                M[pivot][c] = tmp;
+            }
+        }
+        double inv = 1.0 / M[col][col];
+        for (int c = col; c < 5; ++c)
+            M[col][c] *= inv;
+        for (int row = 0; row < 3; ++row) {
+            if (row == col)
+                continue;
+            double factor = M[row][col];
+            if (fabs(factor) < EPSILON)
+                continue;
+            for (int c = col; c < 5; ++c)
+                M[row][c] -= factor * M[col][c];
+        }
+    }
+    return 1;
+}
+
+static void
+static_forces_tetrahedron(struct winch_flex *wf, const struct coord *pos,
+                          double *F)
+{
+    if (!wf->flex_enabled || wf->num_anchors != 4)
+        return;
+    double norm[4] = {0.};
+    double dirs[4][3] = {{0.}};
+    for (int i = 0; i < 3; ++i) {
+        double dx = wf->anchors[i].x - pos->x;
+        double dy = wf->anchors[i].y - pos->y;
+        double dz = wf->anchors[i].z - pos->z;
+        norm[i] = hypot3(dx, dy, dz);
+        if (norm[i] < EPSILON)
+            continue;
+        dirs[i][0] = dx / norm[i];
+        dirs[i][1] = dy / norm[i];
+        dirs[i][2] = dz / norm[i];
+    }
+    double dx = wf->anchors[3].x - pos->x;
+    double dy = wf->anchors[3].y - pos->y;
+    double dz = wf->anchors[3].z - pos->z;
+    double normD = hypot3(dx, dy, dz);
+    double dirD[3] = {0., 0., 0.};
+    if (normD >= EPSILON) {
+        dirD[0] = dx / normD;
+        dirD[1] = dy / normD;
+        dirD[2] = dz / normD;
+    }
+
+    double mg = wf->mover_weight * G_ACCEL;
+    if (mg < EPSILON)
+        return;
+
+    double D_mg = 0., D_pre = 0.;
+    if (wf->anchors[3].z > pos->z && fabs(dirD[2]) >= EPSILON) {
+        D_mg = mg / dirD[2];
+        D_pre = wf->target_force;
+    }
+
+    double M[3][5] = {{0.}};
+    for (int axis = 0; axis < 3; ++axis) {
+        M[axis][0] = dirs[0][axis];
+        M[axis][1] = dirs[1][axis];
+        M[axis][2] = dirs[2][axis];
+        M[axis][3] = -D_mg * dirD[axis];
+        M[axis][4] = -D_pre * dirD[axis];
+    }
+    M[2][3] += mg;
+
+    if (!gauss_jordan_3x5(M))
+        return;
+
+    double A_mg = M[0][3], B_mg = M[1][3], C_mg = M[2][3];
+    double A_pre = M[0][4], B_pre = M[1][4], C_pre = M[2][4];
+
+    double lower = 0.;
+    lower = fmax(lower, fabs(safe_ratio(wf->target_force - A_mg, A_pre)));
+    lower = fmax(lower, fabs(safe_ratio(wf->target_force - B_mg, B_pre)));
+    lower = fmax(lower, fabs(safe_ratio(wf->target_force - C_mg, C_pre)));
+
+    double upper = fabs(safe_ratio(wf->max_force[3] - D_mg, D_pre));
+    upper = fmin(upper, fabs(safe_ratio(wf->max_force[0] - A_mg, A_pre)));
+    upper = fmin(upper, fabs(safe_ratio(wf->max_force[1] - B_mg, B_pre)));
+    upper = fmin(upper, fabs(safe_ratio(wf->max_force[2] - C_mg, C_pre)));
+
+    double preFac = fmin(lower, upper);
+    if (!isfinite(preFac))
+        preFac = 0.;
+
+    double total[4];
+    total[0] = A_mg + preFac * A_pre;
+    total[1] = B_mg + preFac * B_pre;
+    total[2] = C_mg + preFac * C_pre;
+    total[3] = D_mg + preFac * D_pre;
+
+    for (int i = 0; i < 4; ++i)
+        F[i] = clampd(total[i], wf->min_force[i], wf->max_force[i]);
+}
+
+static void
+static_forces_quadrilateral(struct winch_flex *wf, const struct coord *pos,
+                            double *F)
+{
+    if (!wf->flex_enabled || wf->num_anchors != 5)
+        return;
+
+    double mg = wf->mover_weight * G_ACCEL;
+    if (mg < EPSILON)
+        return;
+
+    double norm[5] = {0.};
+    double dirs[5][3] = {{0.}};
+    for (int i = 0; i < 5; ++i) {
+        double dx = wf->anchors[i].x - pos->x;
+        double dy = wf->anchors[i].y - pos->y;
+        double dz = wf->anchors[i].z - pos->z;
+        norm[i] = hypot3(dx, dy, dz);
+        if (norm[i] < EPSILON)
+            continue;
+        dirs[i][0] = dx / norm[i];
+        dirs[i][1] = dy / norm[i];
+        dirs[i][2] = dz / norm[i];
+    }
+
+    double top_mg = 0.;
+    double top_pre = wf->target_force;
+    if (wf->anchors[4].z > pos->z && fabs(dirs[4][2]) >= EPSILON)
+        top_mg = mg / dirs[4][2];
+    else
+        top_pre = 0.;
+
+    double matrices[4][3][5];
+    memset(matrices, 0, sizeof(matrices));
+    for (int i = 0; i < 4; ++i) {
+        for (int axis = 0; axis < 3; ++axis) {
+            for (int skip = 0; skip < 4; ++skip) {
+                if (skip == i)
+                    continue;
+                int col = (i > skip) ? i - 1 : i;
+                matrices[skip][axis][col] = dirs[i][axis];
+            }
+        }
+    }
+    for (int axis = 0; axis < 3; ++axis) {
+        double top_dir = dirs[4][axis];
+        for (int k = 0; k < 4; ++k) {
+            matrices[k][axis][3] = -top_mg * top_dir;
+            matrices[k][axis][4] = -top_pre * top_dir;
+        }
+    }
+    for (int k = 0; k < 4; ++k)
+        matrices[k][2][3] += mg;
+
+    for (int k = 0; k < 4; ++k) {
+        if (!gauss_jordan_3x5(matrices[k]))
+            return;
+    }
+
+    double norm_ABCD[4];
+    for (int k = 0; k < 4; ++k) {
+        double sum = 0.;
+        for (int axis = 0; axis < 3; ++axis) {
+            double v = matrices[k][axis][4];
+            sum += v * v;
+        }
+        norm_ABCD[k] = sqrt(sum);
+        if (norm_ABCD[k] < EPSILON)
+            norm_ABCD[k] = 1.;
+    }
+
+    double p[4] = {0., 0., 0., 0.};
+    double m_vec[4] = {0., 0., 0., 0.};
+    for (int axis = 0; axis < 3; ++axis) {
+        for (int j = 0; j < 4; ++j) {
+            double pt_weight = (wf->target_force >= EPSILON)
+                ? wf->target_force / norm_ABCD[j] : 0.;
+            double mg_weight = 0.25;
+            int s = (j <= axis) ? axis + 1 : axis;
+            p[s] += matrices[j][axis][4] * pt_weight;
+            m_vec[s] += matrices[j][axis][3] * mg_weight;
+        }
+    }
+
+    double lower = 0.;
+    for (int i = 0; i < 4; ++i)
+        lower = fmax(lower, fabs(safe_ratio(wf->target_force - m_vec[i], p[i])));
+
+    double upper = fabs(safe_ratio(wf->max_force[4] - top_mg, top_pre));
+    for (int i = 0; i < 4; ++i)
+        upper = fmin(upper, fabs(safe_ratio(wf->max_force[i] - m_vec[i], p[i])));
+
+    double preFac = fmin(lower, upper);
+    if (!isfinite(preFac))
+        preFac = 0.;
+
+    double total[5];
+    for (int i = 0; i < 4; ++i)
+        total[i] = m_vec[i] + preFac * p[i];
+    total[4] = top_mg + preFac * top_pre;
+
+    for (int i = 0; i < 5; ++i)
+        F[i] = clampd(total[i], wf->min_force[i], wf->max_force[i]);
+}
+
+static void
+static_forces(struct winch_flex *wf, const struct coord *pos, double *F)
+{
+    memset(F, 0, sizeof(double) * wf->num_anchors);
+    if (!wf->flex_enabled)
+        return;
+    if (wf->num_anchors == 4)
+        static_forces_tetrahedron(wf, pos, F);
+    else if (wf->num_anchors == 5)
+        static_forces_quadrilateral(wf, pos, F);
+}
+
+static void
+compute_flex(struct winch_flex *wf, double x, double y, double z,
+             double *distances, double *flex)
+{
+    int num = wf->num_anchors;
+    struct coord pos = { x, y, z };
+    for (int i = 0; i < num; ++i) {
+        double dx = wf->anchors[i].x - x;
+        double dy = wf->anchors[i].y - y;
+        double dz = wf->anchors[i].z - z;
+        distances[i] = hypot3(dx, dy, dz);
+    }
+    if (!wf->flex_enabled) {
+        for (int i = 0; i < num; ++i)
+            flex[i] = 0.;
+        return;
+    }
+    double forces[WINCH_MAX_ANCHORS];
+    static_forces(wf, &pos, forces);
+    for (int i = 0; i < num; ++i) {
+        double spring_length = distances[i] + wf->guy_wires[i];
+        if (spring_length < EPSILON)
+            spring_length = EPSILON;
+        double spring_k = wf->spring_constant / spring_length;
+        double relaxed = distances[i] - forces[i] / spring_k;
+        double line_pos = relaxed - wf->relaxed_origin[i];
+        double delta = distances[i] - wf->distances_origin[i];
+        flex[i] = line_pos - delta;
+    }
+}
+
+static void
+update_cache(struct winch_flex *wf, struct move *m, double move_time)
+{
+    if (!wf || wf->num_anchors <= 0)
+        return;
+    if (wf->cache_valid && wf->cache_move == m && wf->cache_time == move_time)
+        return;
+    struct coord pos = move_get_coord(m, move_time);
+    compute_flex(wf, pos.x, pos.y, pos.z, wf->cache_distances, wf->cache_flex);
+    wf->cache_move = m;
+    wf->cache_time = move_time;
+    wf->cache_valid = 1;
+}
+
+static double
+calc_position_common(struct winch_stepper *ws, struct move *m, double move_time)
+{
+    struct winch_flex *wf = ws->wf;
+    if (!wf || ws->index >= wf->num_anchors)
+        return 0.;
+    update_cache(wf, m, move_time);
+    return wf->cache_distances[ws->index] - wf->cache_flex[ws->index];
+}
+
+static double
+winch_stepper_calc_position(struct stepper_kinematics *sk, struct move *m,
+                            double move_time)
+{
+    struct winch_stepper *ws = container_of(sk, struct winch_stepper, sk);
+    return calc_position_common(ws, m, move_time);
+}
+
+struct winch_flex * __visible
+winch_flex_alloc(void)
+{
+    struct winch_flex *wf = malloc(sizeof(*wf));
+    if (!wf)
+        return NULL;
+    memset(wf, 0, sizeof(*wf));
+    return wf;
+}
+
+void __visible
+winch_flex_free(struct winch_flex *wf)
+{
+    free(wf);
+}
+
+static void
+recalc_origin(struct winch_flex *wf)
+{
+    int num = wf->num_anchors;
+    if (num <= 0)
+        return;
+
+    if (!wf->guy_wires_explicit && num >= 2) {
+        int top = num - 1;
+        for (int i = 0; i < num; ++i) {
+            if (i == top) {
+                wf->guy_wires[i] = 0.;
+                continue;
+            }
+            double dx = wf->anchors[i].x - wf->anchors[top].x;
+            double dy = wf->anchors[i].y - wf->anchors[top].y;
+            double dz = wf->anchors[i].z - wf->anchors[top].z;
+            wf->guy_wires[i] = hypot3(dx, dy, dz);
+        }
+    }
+
+    for (int i = 0; i < num; ++i) {
+        double dx = wf->anchors[i].x;
+        double dy = wf->anchors[i].y;
+        double dz = wf->anchors[i].z;
+        wf->distances_origin[i] = hypot3(dx, dy, dz);
+    }
+
+    if (!wf->flex_enabled) {
+        for (int i = 0; i < num; ++i)
+            wf->relaxed_origin[i] = wf->distances_origin[i];
+        return;
+    }
+
+    struct coord origin = {0., 0., 0.};
+    double forces[WINCH_MAX_ANCHORS];
+    static_forces(wf, &origin, forces);
+
+    for (int i = 0; i < num; ++i) {
+        double spring_length = wf->distances_origin[i] + wf->guy_wires[i];
+        if (spring_length < EPSILON)
+            spring_length = EPSILON;
+        double spring_k = wf->spring_constant / spring_length;
+        double relaxed = wf->distances_origin[i] - forces[i] / spring_k;
+        wf->relaxed_origin[i] = relaxed;
+    }
+}
+
+void __visible
+winch_flex_configure(struct winch_flex *wf, int num_anchors,
+                     const double *anchors, double mover_weight,
+                     double spring_constant, double target_force,
+                     const double *min_force, const double *max_force,
+                     const double *guy_wires, int guy_wires_valid)
+{
+    if (!wf)
+        return;
+    if (num_anchors < 0)
+        num_anchors = 0;
+    if (num_anchors > WINCH_MAX_ANCHORS)
+        num_anchors = WINCH_MAX_ANCHORS;
+    wf->num_anchors = num_anchors;
+    wf->mover_weight = mover_weight;
+    wf->spring_constant = spring_constant;
+    wf->target_force = target_force;
+    wf->guy_wires_explicit = guy_wires_valid;
+
+    for (int i = 0; i < num_anchors; ++i) {
+        wf->anchors[i].x = anchors[i * 3];
+        wf->anchors[i].y = anchors[i * 3 + 1];
+        wf->anchors[i].z = anchors[i * 3 + 2];
+        wf->min_force[i] = min_force ? min_force[i] : 0.;
+        wf->max_force[i] = max_force ? max_force[i] : 1e6;
+        if (guy_wires_valid && guy_wires)
+            wf->guy_wires[i] = guy_wires[i] < 0. ? 0. : guy_wires[i];
+        else
+            wf->guy_wires[i] = 0.;
+    }
+    for (int i = num_anchors; i < WINCH_MAX_ANCHORS; ++i) {
+        wf->anchors[i].x = wf->anchors[i].y = wf->anchors[i].z = 0.;
+        wf->min_force[i] = 0.;
+        wf->max_force[i] = 1e6;
+        wf->guy_wires[i] = 0.;
+    }
+
+    wf->flex_enabled = (num_anchors >= 4
+                        && mover_weight > 0.
+                        && spring_constant > 0.);
+    wf->cache_valid = 0;
+    recalc_origin(wf);
+}
+
+void __visible
+winch_flex_calc_arrays(struct winch_flex *wf, double x, double y, double z,
+                       double *distances_out, double *flex_out)
+{
+    if (!wf || wf->num_anchors <= 0)
+        return;
+    compute_flex(wf, x, y, z, distances_out, flex_out);
+}
+
+struct stepper_kinematics * __visible
+winch_stepper_alloc(struct winch_flex *wf, int index)
+{
+    struct winch_stepper *ws = malloc(sizeof(*ws));
+    if (!ws)
+        return NULL;
+    memset(ws, 0, sizeof(*ws));
+    ws->wf = wf;
+    ws->index = index;
+    ws->sk.calc_position_cb = winch_stepper_calc_position;
+    ws->sk.active_flags = AF_X | AF_Y | AF_Z;
+    return &ws->sk;
 }

klippy/kinematics/winch.py

@@ -3,7 +3,57 @@
 # Copyright (C) 2018-2021  Kevin O'Connor <kevin@koconnor.net>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
-import stepper, mathutil
+import stepper, mathutil, chelper
+
+class WinchFlexHelper:
+    def __init__(self, anchors, config):
+        self.ffi_main, self.ffi_lib = chelper.get_ffi()
+        self.num = len(anchors)
+        self.ptr = self.ffi_main.NULL
+        self.enabled = False
+        if not self.num:
+            return
+        self.ptr = self.ffi_main.gc(
+            self.ffi_lib.winch_flex_alloc(), self.ffi_lib.winch_flex_free)
+        self._configure(anchors, config)
+
+    def _configure(self, anchors, config):
+        mover_weight = config.getfloat('winch_mover_weight', 0., minval=0.)
+        spring_constant = config.getfloat('winch_spring_constant', 0., minval=0.)
+        target_force = config.getfloat('winch_target_force', 0., minval=0.)
+        min_force = list(config.getfloatlist(
+            'winch_min_force', [0.] * self.num, count=self.num))
+        max_force = list(config.getfloatlist(
+            'winch_max_force', [120.] * self.num, count=self.num))
+        guy_raw = config.get('winch_guy_wire_lengths', None, note_valid=False)
+        guy_valid = 0
+        guy_ptr = self.ffi_main.NULL
+        if guy_raw is not None:
+            guy_vals = list(config.getfloatlist(
+                'winch_guy_wire_lengths', count=self.num))
+            guy_ptr = self.ffi_main.new("double[]", guy_vals)
+            guy_valid = 1
+        anchors_flat = [float(coord) for anchor in anchors for coord in anchor]
+        anchors_c = self.ffi_main.new("double[]", anchors_flat)
+        min_c = self.ffi_main.new("double[]", min_force)
+        max_c = self.ffi_main.new("double[]", max_force)
+        self.ffi_lib.winch_flex_configure(
+            self.ptr, self.num, anchors_c, mover_weight, spring_constant,
+            target_force, min_c, max_c, guy_ptr, guy_valid)
+        self.enabled = bool(self.num >= 4 and mover_weight > 0. and spring_constant > 0.)
+
+    def get_ptr(self):
+        return self.ptr
+
+    def calc_arrays(self, pos):
+        if not self.ptr or not self.num:
+            return [], []
+        distances = self.ffi_main.new("double[]", self.num)
+        flex = self.ffi_main.new("double[]", self.num)
+        self.ffi_lib.winch_flex_calc_arrays(self.ptr, pos[0], pos[1], pos[2],
+                                            distances, flex)
+        return ([distances[i] for i in range(self.num)],
+                [flex[i] for i in range(self.num)])
 
 class WinchKinematics:
     def __init__(self, toolhead, config):
@@ -17,9 +67,12 @@ class WinchKinematics:
             stepper_config = config.getsection(name)
             s = stepper.PrinterStepper(stepper_config)
             self.steppers.append(s)
-            a = tuple([stepper_config.getfloat('anchor_' + n) for n in 'xyz'])
+            a = tuple(stepper_config.getfloat('anchor_' + n) for n in 'xyz')
             self.anchors.append(a)
-            s.setup_itersolve('winch_stepper_alloc', *a)
+        self.flex_helper = WinchFlexHelper(self.anchors, config)
+        flex_ptr = self.flex_helper.get_ptr()
+        for idx, s in enumerate(self.steppers):
+            s.setup_itersolve('winch_stepper_alloc', flex_ptr, idx)
             s.set_trapq(toolhead.get_trapq())
         # Setup boundary checks
         acoords = list(zip(*self.anchors))
@@ -31,7 +84,18 @@ class WinchKinematics:
     def calc_position(self, stepper_positions):
         # Use only first three steppers to calculate cartesian position
         pos = [stepper_positions[rail.get_name()] for rail in self.steppers[:3]]
-        return mathutil.trilateration(self.anchors[:3], [sp*sp for sp in pos])
+        cart = mathutil.trilateration(self.anchors[:3], [sp * sp for sp in pos])
+        if not self.flex_helper.enabled:
+            return cart
+        cart_guess = list(cart)
+        for _ in range(3):
+            distances, flex = self.flex_helper.calc_arrays(cart_guess)
+            if len(distances) < 3:
+                break
+            adjusted = [pos[i] + flex[i] for i in range(3)]
+            cart_guess = list(mathutil.trilateration(
+                self.anchors[:3], [d * d for d in adjusted]))
+        return cart_guess
     def set_position(self, newpos, homing_axes):
         for s in self.steppers:
             s.set_position(newpos)