From bc9e5b2a0cdb2ae68d06872aba5eb88c54b9e10c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Torbj=C3=B8rn=20Ludvigsen?= Date: Fri, 10 Oct 2025 23:11:43 +0200 Subject: [PATCH] feat: Flex compensation for Hangprinters --- config/example-winch.cfg | 6 + docs/Config_Reference.md | 21 ++ klippy/chelper/__init__.py | 13 +- klippy/chelper/kin_winch.c | 502 +++++++++++++++++++++++++++++++++++-- klippy/kinematics/winch.py | 72 +++++- 5 files changed, 586 insertions(+), 28 deletions(-) diff --git a/config/example-winch.cfg b/config/example-winch.cfg index d6d62b8a6..c2ba3e09c 100644 --- a/config/example-winch.cfg +++ b/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 diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index 94d3a9730..b3da8de51 100644 --- a/docs/Config_Reference.md +++ b/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 diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index c26196e66..66b03f576 100644 --- a/klippy/chelper/__init__.py +++ b/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 = """ diff --git a/klippy/chelper/kin_winch.c b/klippy/chelper/kin_winch.c index c6501d270..ad09b6d83 100644 --- a/klippy/chelper/kin_winch.c +++ b/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 +// Copyright (C) 2024 Contributors // // This file may be distributed under the terms of the GNU GPLv3 license. -#include // sqrt +#include // sqrt, fabs, fmax, fmin #include // offsetof #include // malloc #include // 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; } diff --git a/klippy/kinematics/winch.py b/klippy/kinematics/winch.py index 2fa06ef22..8bb236e36 100644 --- a/klippy/kinematics/winch.py +++ b/klippy/kinematics/winch.py @@ -3,7 +3,57 @@ # Copyright (C) 2018-2021 Kevin O'Connor # # 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)