diff options
Diffstat (limited to 'klippy/chelper/itersolve.c')
| -rw-r--r-- | klippy/chelper/itersolve.c | 179 |
1 files changed, 79 insertions, 100 deletions
diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c index 36f9ea90..0dbc6c51 100644 --- a/klippy/chelper/itersolve.c +++ b/klippy/chelper/itersolve.c @@ -22,52 +22,6 @@ struct timepos { double time, position; }; -// Find step using "false position" method (with "Illinois algorithm") -static struct timepos -itersolve_find_step(struct stepper_kinematics *sk, struct move *m - , struct timepos low, struct timepos high - , double target) -{ - sk_calc_callback calc_position_cb = sk->calc_position_cb; - struct timepos best_guess = high; - low.position -= target; - high.position -= target; - if (!high.position) - // The high range was a perfect guess for the next step - return best_guess; - int high_sign = signbit(high.position); - if (high_sign == signbit(low.position)) - // The target is not in the low/high range - return low range - return (struct timepos){ low.time, target }; - int prev_choice = 0; - for (;;) { - double guess_time = ((low.time*high.position - high.time*low.position) - / (high.position - low.position)); - best_guess.time = guess_time; - best_guess.position = calc_position_cb(sk, m, guess_time); - double guess_position = best_guess.position - target; - if (fabs(guess_position) <= .000000001) - break; - int guess_sign = signbit(guess_position); - if (guess_sign == high_sign) { - high.time = guess_time; - high.position = guess_position; - if (prev_choice > 0) - low.position *= .5; - prev_choice = 1; - } else { - low.time = guess_time; - low.position = guess_position; - if (prev_choice < 0) - high.position *= .5; - prev_choice = -1; - } - if (high.time - low.time <= .000000001) - break; - } - return best_guess; -} - #define SEEK_TIME_RESET 0.000100 // Generate step times for a portion of a move @@ -82,67 +36,92 @@ itersolve_gen_steps_range(struct stepper_kinematics *sk, struct move *m start = 0.; if (end > m->move_t) end = m->move_t; - struct timepos last = { start, sk->commanded_pos }, low = last, high = last; - double seek_time_delta = SEEK_TIME_RESET; - int sdir = stepcompress_get_step_dir(sk->sc), is_dir_change = 0; + struct timepos old_guess = {start, sk->commanded_pos}, guess = old_guess; + int sdir = stepcompress_get_step_dir(sk->sc); + int is_dir_change = 0, have_bracket = 0, check_oscillate = 0; + double target = sk->commanded_pos + (sdir ? half_step : -half_step); + double last_time=start, low_time=start, high_time=start + SEEK_TIME_RESET; + if (high_time > end) + high_time = end; for (;;) { - double diff = high.position - last.position, dist = sdir ? diff : -diff; - if (dist >= half_step) { - // Have valid upper bound - now find step - double target = last.position + (sdir ? half_step : -half_step); - struct timepos next = itersolve_find_step(sk, m, low, high, target); - // Add step at given time - int ret = stepcompress_append(sk->sc, sdir - , m->print_time, next.time); - if (ret) - return ret; - seek_time_delta = next.time - last.time; - if (seek_time_delta < .000000001) - seek_time_delta = .000000001; - if (is_dir_change && seek_time_delta > SEEK_TIME_RESET) - seek_time_delta = SEEK_TIME_RESET; - is_dir_change = 0; - last.position = target + (sdir ? half_step : -half_step); - last.time = next.time; - low = next; - if (low.time < high.time) - // The existing search range is still valid - continue; - } else if (dist > 0.) { - // Avoid rollback if stepper fully reaches target position - stepcompress_commit(sk->sc); - } else if (unlikely(dist < -(half_step + .000000001))) { - // Found direction change - is_dir_change = 1; - if (seek_time_delta > SEEK_TIME_RESET) - seek_time_delta = SEEK_TIME_RESET; - if (low.time > last.time) { - // Update direction and retry + // Use the "secant method" to guess a new time from previous guesses + double guess_dist = guess.position - target; + double og_dist = old_guess.position - target; + double next_time = ((old_guess.time*guess_dist - guess.time*og_dist) + / (guess_dist - og_dist)); + if (!(next_time > low_time && next_time < high_time)) { // or NaN + // Next guess is outside bounds checks - validate it + if (have_bracket) { + // A poor guess - fall back to bisection + next_time = (low_time + high_time) * .5; + check_oscillate = 0; + } else if (guess.time >= end) { + // No more steps present in requested time range + break; + } else { + // Might be a poor guess - limit to exponential search + next_time = high_time; + high_time = 2. * high_time - last_time; + if (high_time > end) + high_time = end; + } + } + // Calculate position at next_time guess + old_guess = guess; + guess.time = next_time; + guess.position = calc_position_cb(sk, m, next_time); + guess_dist = guess.position - target; + if (fabs(guess_dist) > .000000001) { + // Guess does not look close enough - update bounds + double rel_dist = sdir ? guess_dist : -guess_dist; + if (rel_dist > 0.) { + // Found position past target, so step is definitely present + if (have_bracket && old_guess.time <= low_time) { + if (check_oscillate) + // Force bisect next to avoid persistent oscillations + old_guess = guess; + check_oscillate = 1; + } + high_time = guess.time; + have_bracket = 1; + } else if (rel_dist < -(half_step + half_step + .000000010)) { + // Found direction change sdir = !sdir; - continue; + target = (sdir ? target + half_step + half_step + : target - half_step - half_step); + low_time = last_time; + high_time = guess.time; + is_dir_change = have_bracket = 1; + check_oscillate = 0; + } else { + low_time = guess.time; } - // Must update range to avoid re-finding previous time - if (high.time > last.time + .000000001) { - // Reduce the high bound - it will become a better low bound - high.time = (last.time + high.time) * .5; - high.position = calc_position_cb(sk, m, high.time); + if (!have_bracket || high_time - low_time > .000000001) { + if (!is_dir_change && rel_dist >= -half_step) + // Avoid rollback if stepper fully reaches step position + stepcompress_commit(sk->sc); + // Guess is not close enough - guess again with new time continue; } } - // Need to increase the search range to find an upper bound - if (high.time >= end) - // At end of move - break; - low = high; - do { - high.time = last.time + seek_time_delta; - seek_time_delta += seek_time_delta; - } while (unlikely(high.time <= low.time)); - if (high.time > end) - high.time = end; - high.position = calc_position_cb(sk, m, high.time); + // Found next step - submit it + int ret = stepcompress_append(sk->sc, sdir, m->print_time, guess.time); + if (ret) + return ret; + target = sdir ? target+half_step+half_step : target-half_step-half_step; + // Reset bounds checking + double seek_time_delta = 1.5 * (guess.time - last_time); + if (seek_time_delta < .000000001) + seek_time_delta = .000000001; + if (is_dir_change && seek_time_delta > SEEK_TIME_RESET) + seek_time_delta = SEEK_TIME_RESET; + last_time = low_time = guess.time; + high_time = guess.time + seek_time_delta; + if (high_time > end) + high_time = end; + is_dir_change = have_bracket = check_oscillate = 0; } - sk->commanded_pos = last.position; + sk->commanded_pos = target - (sdir ? half_step : -half_step); if (sk->post_cb) sk->post_cb(sk); return 0; |