3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

itersolve: Convert iterative solver to use "secant method" 

The previous code calculates each step time via an "exponential
search" followed by a "false position with Illinois algorithm" search.
Replace with a "secant method" with "bounds check" search.  This
simplifies the code, improves the performance, and does a better job
of finding steps near a direction change.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2020-09-21 12:03:09 -04:00
parent e0842e0e03
commit ccc8fe2fc5
1 changed files with 80 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

181
klippy/chelper/itersolve.c
View File

@ -22,52 +22,6 @@ struct timepos {
double time, position; 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 #define SEEK_TIME_RESET 0.000100
// Generate step times for a portion of a move // 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.; start = 0.;
if (end > m->move_t) if (end > m->move_t)
end = m->move_t; end = m->move_t;
struct timepos last = { start, sk->commanded_pos }, low = last, high = last; struct timepos old_guess = {start, sk->commanded_pos}, guess = old_guess;
double seek_time_delta = SEEK_TIME_RESET; int sdir = stepcompress_get_step_dir(sk->sc);
int sdir = stepcompress_get_step_dir(sk->sc), is_dir_change = 0; 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 (;;) { for (;;) {
double diff = high.position - last.position, dist = sdir ? diff : -diff; // Use the "secant method" to guess a new time from previous guesses
if (dist >= half_step) { double guess_dist = guess.position - target;
// Have valid upper bound - now find step double og_dist = old_guess.position - target;
double target = last.position + (sdir ? half_step : -half_step); double next_time = ((old_guess.time*guess_dist - guess.time*og_dist)
struct timepos next = itersolve_find_step(sk, m, low, high, target); / (guess_dist - og_dist));
// Add step at given time if (!(next_time > low_time && next_time < high_time)) { // or NaN
int ret = stepcompress_append(sk->sc, sdir // Next guess is outside bounds checks - validate it
, m->print_time, next.time); if (have_bracket) {
if (ret) // A poor guess - fall back to bisection
return ret; next_time = (low_time + high_time) * .5;
seek_time_delta = next.time - last.time; check_oscillate = 0;
if (seek_time_delta < .000000001) } else if (guess.time >= end) {
seek_time_delta = .000000001; // No more steps present in requested time range
if (is_dir_change && seek_time_delta > SEEK_TIME_RESET) break;
seek_time_delta = SEEK_TIME_RESET; } else {
is_dir_change = 0; // Might be a poor guess - limit to exponential search
last.position = target + (sdir ? half_step : -half_step); next_time = high_time;
last.time = next.time; high_time = 2. * high_time - last_time;
low = next; if (high_time > end)
if (low.time < high.time) high_time = end;
// 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
sdir = !sdir;
continue;
} }
// Must update range to avoid re-finding previous time }
if (high.time > last.time + .000000001) { // Calculate position at next_time guess
// Reduce the high bound - it will become a better low bound old_guess = guess;
high.time = (last.time + high.time) * .5; guess.time = next_time;
high.position = calc_position_cb(sk, m, high.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;
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;
}
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; continue;
} }
} }
// Need to increase the search range to find an upper bound // Found next step - submit it
if (high.time >= end) int ret = stepcompress_append(sk->sc, sdir, m->print_time, guess.time);
// At end of move if (ret)
break; return ret;
low = high; target = sdir ? target+half_step+half_step : target-half_step-half_step;
do { // Reset bounds checking
high.time = last.time + seek_time_delta; double seek_time_delta = 1.5 * (guess.time - last_time);
seek_time_delta += seek_time_delta; if (seek_time_delta < .000000001)
} while (unlikely(high.time <= low.time)); seek_time_delta = .000000001;
if (high.time > end) if (is_dir_change && seek_time_delta > SEEK_TIME_RESET)
high.time = end; seek_time_delta = SEEK_TIME_RESET;
high.position = calc_position_cb(sk, m, high.time); 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) if (sk->post_cb)
sk->post_cb(sk); sk->post_cb(sk);
return 0; return 0;