diff options
| -rw-r--r-- | klippy/chelper/__init__.py | 4 | ||||
| -rw-r--r-- | klippy/chelper/kin_cartesian.c | 119 | ||||
| -rw-r--r-- | klippy/chelper/stepcompress.c | 2 | ||||
| -rw-r--r-- | klippy/chelper/stepcompress.h | 3 | ||||
| -rw-r--r-- | klippy/mcu.py | 24 | ||||
| -rw-r--r-- | klippy/stepper.py | 8 |
6 files changed, 6 insertions, 154 deletions
diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 51f59e59..fd8b60a6 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -55,10 +55,6 @@ defs_itersolve = """ """ defs_kin_cartesian = """ - int32_t stepcompress_push(struct stepcompress *sc, double step_clock - , int32_t sdir); - int32_t stepcompress_push_const(struct stepcompress *sc, double clock_offset - , double step_offset, double steps, double start_sv, double accel); struct stepper_kinematics *cartesian_stepper_alloc(char axis); """ diff --git a/klippy/chelper/kin_cartesian.c b/klippy/chelper/kin_cartesian.c index c9d01e0a..2b323d14 100644 --- a/klippy/chelper/kin_cartesian.c +++ b/klippy/chelper/kin_cartesian.c @@ -1,129 +1,14 @@ // Cartesian kinematics stepper pulse time generation // -// Copyright (C) 2016-2018 Kevin O'Connor <kevin@koconnor.net> +// Copyright (C) 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 <stdlib.h> // malloc #include <string.h> // memset -#include "compiler.h" // likely +#include "compiler.h" // __visible #include "itersolve.h" // move_get_coord #include "pyhelper.h" // errorf -#include "stepcompress.h" // queue_append - - -/**************************************************************** - * Direct step generation - ****************************************************************/ - -// Common suffixes: _sd is step distance (a unit length the same -// distance the stepper moves on each step), _sv is step velocity (in -// units of step distance per time), _sd2 is step distance squared, _r -// is ratio (scalar usually between 0.0 and 1.0). Times are in -// seconds and acceleration is in units of step distance per second -// squared. - -// Wrapper around sqrt() to handle small negative numbers -static double -_safe_sqrt(double v) -{ - // Due to floating point truncation, it's possible to get a small - // negative number - treat it as zero. - if (v < -0.001) - errorf("safe_sqrt of %.9f", v); - return 0.; -} -inline double safe_sqrt(double v) { - return likely(v >= 0.) ? sqrt(v) : _safe_sqrt(v); -} - -// Schedule a step event at the specified step_clock time -int32_t __visible -stepcompress_push(struct stepcompress *sc, double print_time, int32_t sdir) -{ - int ret = set_next_step_dir(sc, !!sdir); - if (ret) - return ret; - struct queue_append qa = queue_append_start(sc, print_time, 0.5); - ret = queue_append(&qa, 0.); - if (ret) - return ret; - queue_append_finish(qa); - return sdir ? 1 : -1; -} - -// Schedule 'steps' number of steps at constant acceleration. If -// acceleration is zero (ie, constant velocity) it uses the formula: -// step_time = print_time + step_num/start_sv -// Otherwise it uses the formula: -// step_time = (print_time + sqrt(2*step_num/accel + (start_sv/accel)**2) -// - start_sv/accel) -int32_t __visible -stepcompress_push_const( - struct stepcompress *sc, double print_time - , double step_offset, double steps, double start_sv, double accel) -{ - // Calculate number of steps to take - int sdir = 1; - if (steps < 0) { - sdir = 0; - steps = -steps; - step_offset = -step_offset; - } - int count = steps + .5 - step_offset; - if (count <= 0 || count > 10000000) { - if (count && steps) { - errorf("push_const invalid count %d %f %f %f %f %f" - , stepcompress_get_oid(sc), print_time, step_offset, steps - , start_sv, accel); - 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 - 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; - double pos = (step_offset + .5) * inv_cruise_sv; - while (count--) { - ret = queue_append(&qa, pos); - if (ret) - return ret; - pos += inv_cruise_sv; - } - queue_append_finish(qa); - } else { - // Move with constant acceleration - double inv_accel = 1. / accel; - double mcu_freq = stepcompress_get_mcu_freq(sc); - double accel_time = start_sv * inv_accel * mcu_freq; - struct queue_append qa = queue_append_start( - sc, print_time, 0.5 - accel_time); - double accel_multiplier = 2. * inv_accel * mcu_freq * mcu_freq; - double pos = (step_offset + .5)*accel_multiplier + accel_time*accel_time; - while (count--) { - double v = safe_sqrt(pos); - int ret = queue_append(&qa, accel_multiplier >= 0. ? v : -v); - if (ret) - return ret; - pos += accel_multiplier; - } - queue_append_finish(qa); - } - return res; -} - - -/**************************************************************** - * Iterative solver - ****************************************************************/ static double cart_stepper_x_calc_position(struct stepper_kinematics *sk, struct move *m diff --git a/klippy/chelper/stepcompress.c b/klippy/chelper/stepcompress.c index 788965ba..6431793b 100644 --- a/klippy/chelper/stepcompress.c +++ b/klippy/chelper/stepcompress.c @@ -313,7 +313,7 @@ stepcompress_flush_far(struct stepcompress *sc, uint64_t abs_step_clock) } // Send the set_next_step_dir command -int +static int set_next_step_dir(struct stepcompress *sc, int sdir) { if (sc->sdir == sdir) diff --git a/klippy/chelper/stepcompress.h b/klippy/chelper/stepcompress.h index bf756143..4e74bbda 100644 --- a/klippy/chelper/stepcompress.h +++ b/klippy/chelper/stepcompress.h @@ -10,7 +10,6 @@ void stepcompress_fill(struct stepcompress *sc, uint32_t max_error , uint32_t invert_sdir, uint32_t queue_step_msgid , uint32_t set_next_step_dir_msgid); void stepcompress_free(struct stepcompress *sc); -int set_next_step_dir(struct stepcompress *sc, int sdir); int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock); int stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock); int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len); @@ -38,6 +37,4 @@ void steppersync_set_time(struct steppersync *ss, double time_offset , double mcu_freq); int steppersync_flush(struct steppersync *ss, uint64_t move_clock); -double safe_sqrt(double v); - #endif // stepcompress.h diff --git a/klippy/mcu.py b/klippy/mcu.py index 820d8a4d..be97dc99 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -26,8 +26,7 @@ class MCU_stepper: self._stepqueue = ffi_main.gc(self._ffi_lib.stepcompress_alloc(oid), self._ffi_lib.stepcompress_free) self._mcu.register_stepqueue(self._stepqueue) - self._stepcompress_push_const = self._itersolve_gen_steps = None - self._stepper_kinematics = None + self._stepper_kinematics = self._itersolve_gen_steps = None self.set_ignore_move(False) def get_mcu(self): return self._mcu @@ -88,13 +87,11 @@ class MCU_stepper: return int(mcu_pos + 0.5) return int(mcu_pos - 0.5) def set_ignore_move(self, ignore_move): - was_ignore = (self._stepcompress_push_const - is not self._ffi_lib.stepcompress_push_const) + was_ignore = (self._itersolve_gen_steps + is not self._ffi_lib.itersolve_gen_steps) if ignore_move: - self._stepcompress_push_const = (lambda *args: 0) self._itersolve_gen_steps = (lambda *args: 0) else: - self._stepcompress_push_const = self._ffi_lib.stepcompress_push_const self._itersolve_gen_steps = self._ffi_lib.itersolve_gen_steps return was_ignore def note_homing_start(self, homing_clock): @@ -122,21 +119,6 @@ class MCU_stepper: if self._invert_dir: pos = -pos self._commanded_pos = pos - self._mcu_position_offset - def step(self, print_time, sdir): - count = self._ffi_lib.stepcompress_push( - self._stepqueue, print_time, sdir) - if count == STEPCOMPRESS_ERROR_RET: - raise error("Internal error in stepcompress") - self._commanded_pos += count - def step_const(self, print_time, start_pos, dist, start_v, accel): - inv_step_dist = self._inv_step_dist - step_offset = self._commanded_pos - start_pos * inv_step_dist - count = self._stepcompress_push_const( - self._stepqueue, print_time, step_offset, dist * inv_step_dist, - start_v * inv_step_dist, accel * inv_step_dist) - if count == STEPCOMPRESS_ERROR_RET: - raise error("Internal error in stepcompress") - self._commanded_pos += count def step_itersolve(self, cmove): count = self._itersolve_gen_steps(self._stepper_kinematics, cmove) if count == STEPCOMPRESS_ERROR_RET: diff --git a/klippy/stepper.py b/klippy/stepper.py index 3ea04282..f082f1e4 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -47,8 +47,6 @@ class PrinterStepper: self.mcu_stepper.setup_dir_pin(dir_pin_params) self.step_dist = config.getfloat('step_distance', above=0.) self.mcu_stepper.setup_step_distance(self.step_dist) - self.step = self.mcu_stepper.step - self.step_const = self.mcu_stepper.step_const self.step_itersolve = self.mcu_stepper.step_itersolve self.setup_itersolve = self.mcu_stepper.setup_itersolve self.enable = lookup_enable_pin(ppins, config.get('enable_pin', None)) @@ -185,7 +183,6 @@ class PrinterMultiStepper(PrinterHomingStepper): PrinterHomingStepper.__init__(self, printer, config) self.endstops = PrinterHomingStepper.get_endstops(self) self.extras = [] - self.all_step_const = [self.step_const] self.all_step_itersolve = [self.step_itersolve] for i in range(1, 99): if not config.has_section(config.get_name() + str(i)): @@ -193,7 +190,6 @@ class PrinterMultiStepper(PrinterHomingStepper): extraconfig = config.getsection(config.get_name() + str(i)) extra = PrinterStepper(printer, extraconfig) self.extras.append(extra) - self.all_step_const.append(extra.step_const) self.all_step_itersolve.append(extra.step_itersolve) extraendstop = extraconfig.get('endstop_pin', None) if extraendstop is not None: @@ -203,11 +199,7 @@ class PrinterMultiStepper(PrinterHomingStepper): self.endstops.append((mcu_endstop, extra.name)) else: self.mcu_endstop.add_stepper(extra.mcu_stepper) - self.step_const = self.step_multi_const self.step_itersolve = self.step_multi_itersolve - def step_multi_const(self, print_time, start_pos, dist, start_v, accel): - for step_const in self.all_step_const: - step_const(print_time, start_pos, dist, start_v, accel) def step_multi_itersolve(self, cmove): for step_itersolve in self.all_step_itersolve: step_itersolve(cmove) |