diff --git a/ai_docs/solver_calc_position.c b/ai_docs/solver_calc_position.c index 3d3a49e2b..1ce6ef8d2 100644 --- a/ai_docs/solver_calc_position.c +++ b/ai_docs/solver_calc_position.c @@ -141,6 +141,18 @@ parse_cfg_line(const char *line, struct cfg_state *cfg) return 1; } +static double +gear_ratio(const struct cfg_state *cfg) +{ + double motor_gear = cfg->motor_gear; + if (motor_gear == 0.) + motor_gear = 1.; + double gear = cfg->spool_gear / motor_gear; + if (gear <= 0.) + gear = 1.; + return gear; +} + static double rotation_distance_for_axis(const struct cfg_state *cfg, size_t idx) { @@ -148,8 +160,8 @@ rotation_distance_for_axis(const struct cfg_state *cfg, size_t idx) double ma = cfg->mech_adv[idx]; if (ma == 0.) ma = 1.; - double gear = cfg->spool_gear / cfg->motor_gear; - return (2.0 * M_PI * r) / (gear * ma); + // Use circumference/mechanical advantage; gear ratio is handled via steps_per_rotation. + return (2.0 * M_PI * r) / ma; } static int @@ -179,11 +191,13 @@ solve_sample(const struct cfg_state *cfg, size_t num, int use_flex, mech_adv); winch_flex_set_enabled(wf, use_flex ? 1 : 0); + double gear = gear_ratio(cfg); double motor_mm[WINCH_MAX_ANCHORS]; for (size_t i = 0; i < num; ++i) { double rd = rotation_distance_for_axis(cfg, i); - winch_flex_set_spool_params(wf, (int)i, rd, cfg->steps_per_rev); - motor_mm[i] = motor_deg[i] / cfg->steps_per_rev * rd; + double steps_per_rotation = cfg->steps_per_rev * gear; + winch_flex_set_spool_params(wf, (int)i, rd, steps_per_rotation); + motor_mm[i] = motor_deg[i] / cfg->steps_per_rev * (rd / gear); } struct timespec t0, t1; diff --git a/klippy/chelper/kin_winch.c b/klippy/chelper/kin_winch.c index 059076a61..ed5bb4efd 100644 --- a/klippy/chelper/kin_winch.c +++ b/klippy/chelper/kin_winch.c @@ -610,7 +610,10 @@ compute_flex(struct winch_flex *wf, double x, double y, double z, if (spring_length < EPSILON) spring_length = EPSILON; double spring_k = wf->spring_constant / spring_length; - flex[i] = -forces[i] / spring_k; + double ma = wf->mechanical_advantage[i]; + if (ma <= 0.) + ma = 1.; + flex[i] = -forces[i] / (spring_k * ma); } }