diff options
Diffstat (limited to 'klippy/chelper/kin_delta.c')
| -rw-r--r-- | klippy/chelper/kin_delta.c | 133 |
1 files changed, 133 insertions, 0 deletions
diff --git a/klippy/chelper/kin_delta.c b/klippy/chelper/kin_delta.c new file mode 100644 index 00000000..1925ae20 --- /dev/null +++ b/klippy/chelper/kin_delta.c @@ -0,0 +1,133 @@ +// Delta kinematics stepper pulse time generation +// +// Copyright (C) 2016-2018 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <math.h> // sqrt +#include "compiler.h" // __visible +#include "pyhelper.h" // errorf +#include "stepcompress.h" // queue_append + +// Schedule steps using delta kinematics +static int32_t +_stepcompress_push_delta( + struct stepcompress *sc, int sdir + , double print_time, double move_sd, double start_sv, double accel + , double height, double startxy_sd, double arm_sd, double movez_r) +{ + // Calculate number of steps to take + double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.; + double arm_sd2 = arm_sd * arm_sd; + double endxy_sd = startxy_sd - movexy_r*move_sd; + double end_height = safe_sqrt(arm_sd2 - endxy_sd*endxy_sd); + int count = (end_height + movez_r*move_sd - height) * (sdir ? 1. : -1.) + .5; + if (count <= 0 || count > 10000000) { + if (count) { + errorf("push_delta invalid count %d %d %f %f %f %f %f %f %f %f" + , stepcompress_get_oid(sc), count, print_time, move_sd + , start_sv, accel, height, startxy_sd, arm_sd, movez_r); + return ERROR_RET; + } + return 0; + } + int ret = set_next_step_dir(sc, sdir); + if (ret) + return ret; + int res = sdir ? count : -count; + + // Calculate each step time + height += (sdir ? .5 : -.5); + if (!accel) { + // Move at constant velocity (zero acceleration) + struct queue_append qa = queue_append_start(sc, print_time, .5); + double inv_cruise_sv = stepcompress_get_mcu_freq(sc) / start_sv; + if (!movez_r) { + // Optimized case for common XY only moves (no Z movement) + while (count--) { + double v = safe_sqrt(arm_sd2 - height*height); + double pos = startxy_sd + (sdir ? -v : v); + int ret = queue_append(&qa, pos * inv_cruise_sv); + if (ret) + return ret; + height += (sdir ? 1. : -1.); + } + } else if (!movexy_r) { + // Optimized case for Z only moves + double pos = ((sdir ? height-end_height : end_height-height) + * inv_cruise_sv); + while (count--) { + int ret = queue_append(&qa, pos); + if (ret) + return ret; + pos += inv_cruise_sv; + } + } else { + // General case (handles XY+Z moves) + double start_pos = movexy_r*startxy_sd, zoffset = movez_r*startxy_sd; + while (count--) { + double relheight = movexy_r*height - zoffset; + double v = safe_sqrt(arm_sd2 - relheight*relheight); + double pos = start_pos + movez_r*height + (sdir ? -v : v); + int ret = queue_append(&qa, pos * inv_cruise_sv); + if (ret) + return ret; + height += (sdir ? 1. : -1.); + } + } + queue_append_finish(qa); + } else { + // Move with constant acceleration + double start_pos = movexy_r*startxy_sd, zoffset = movez_r*startxy_sd; + double mcu_freq = stepcompress_get_mcu_freq(sc); + double inv_accel = 1. / accel; + start_pos += 0.5 * start_sv*start_sv * inv_accel; + struct queue_append qa = queue_append_start( + sc, print_time, 0.5 - start_sv * inv_accel * mcu_freq); + double accel_multiplier = 2. * inv_accel * mcu_freq * mcu_freq; + while (count--) { + double relheight = movexy_r*height - zoffset; + double v = safe_sqrt(arm_sd2 - relheight*relheight); + double pos = start_pos + movez_r*height + (sdir ? -v : v); + v = safe_sqrt(pos * accel_multiplier); + int ret = queue_append(&qa, accel_multiplier >= 0. ? v : -v); + if (ret) + return ret; + height += (sdir ? 1. : -1.); + } + queue_append_finish(qa); + } + return res; +} + +int32_t __visible +stepcompress_push_delta( + struct stepcompress *sc, double print_time, double move_sd + , double start_sv, double accel + , double height, double startxy_sd, double arm_sd, double movez_r) +{ + double reversexy_sd = startxy_sd + arm_sd*movez_r; + if (reversexy_sd <= 0.) + // All steps are in down direction + return _stepcompress_push_delta( + sc, 0, print_time, move_sd, start_sv, accel + , height, startxy_sd, arm_sd, movez_r); + double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.; + if (reversexy_sd >= move_sd * movexy_r) + // All steps are in up direction + return _stepcompress_push_delta( + sc, 1, print_time, move_sd, start_sv, accel + , height, startxy_sd, arm_sd, movez_r); + // Steps in both up and down direction + int res1 = _stepcompress_push_delta( + sc, 1, print_time, reversexy_sd / movexy_r, start_sv, accel + , height, startxy_sd, arm_sd, movez_r); + if (res1 == ERROR_RET) + return res1; + int res2 = _stepcompress_push_delta( + sc, 0, print_time, move_sd, start_sv, accel + , height + res1, startxy_sd, arm_sd, movez_r); + if (res2 == ERROR_RET) + return res2; + return res1 + res2; +} |