diff options
| -rw-r--r-- | klippy/chelper/__init__.py | 5 | ||||
| -rw-r--r-- | klippy/chelper/kin_delta.c | 146 | ||||
| -rw-r--r-- | klippy/delta.py | 70 | ||||
| -rw-r--r-- | klippy/mcu.py | 24 | ||||
| -rw-r--r-- | klippy/stepper.py | 3 |
5 files changed, 60 insertions, 188 deletions
diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index da31ec38..36e32303 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -62,9 +62,8 @@ defs_kin_cartesian = """ """ defs_kin_delta = """ - int32_t stepcompress_push_delta(struct stepcompress *sc - , double clock_offset, double move_sd, double start_sv, double accel - , double height, double startxy_sd, double arm_d, double movez_r); + struct stepper_kinematics *delta_stepper_alloc(double arm2 + , double tower_x, double tower_y); """ defs_serialqueue = """ diff --git a/klippy/chelper/kin_delta.c b/klippy/chelper/kin_delta.c index 1925ae20..91794be9 100644 --- a/klippy/chelper/kin_delta.c +++ b/klippy/chelper/kin_delta.c @@ -1,133 +1,39 @@ // Delta 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 <stddef.h> // offsetof +#include <stdlib.h> // malloc +#include <string.h> // memset #include "compiler.h" // __visible -#include "pyhelper.h" // errorf -#include "stepcompress.h" // queue_append +#include "itersolve.h" // struct stepper_kinematics -// 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; +struct delta_stepper { + struct stepper_kinematics sk; + double arm2, tower_x, tower_y; +}; - // 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; +static double +delta_stepper_calc_position(struct stepper_kinematics *sk, struct move *m + , double move_time) +{ + struct delta_stepper *ds = container_of(sk, struct delta_stepper, sk); + struct coord c = move_get_coord(m, move_time); + double dx = ds->tower_x - c.x, dy = ds->tower_y - c.y; + return sqrt(ds->arm2 - dx*dx - dy*dy) + c.z; } -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) +struct stepper_kinematics * __visible +delta_stepper_alloc(double arm2, double tower_x, double tower_y) { - 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; + struct delta_stepper *ds = malloc(sizeof(*ds)); + memset(ds, 0, sizeof(*ds)); + ds->arm2 = arm2; + ds->tower_x = tower_x; + ds->tower_y = tower_y; + ds->sk.calc_position = delta_stepper_calc_position; + return &ds->sk; } diff --git a/klippy/delta.py b/klippy/delta.py index 7a518593..8be21ae4 100644 --- a/klippy/delta.py +++ b/klippy/delta.py @@ -4,7 +4,7 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import math, logging -import stepper, homing +import stepper, homing, chelper StepList = (0, 1, 2) @@ -56,6 +56,14 @@ class DeltaKinematics: self.towers = [(math.cos(math.radians(angle)) * radius, math.sin(math.radians(angle)) * radius) for angle in self.angles] + # Setup iterative solver + ffi_main, ffi_lib = chelper.get_ffi() + self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free) + self.move_fill = ffi_lib.move_fill + for s, a, t in zip(self.steppers, self.arm2, self.towers): + sk = ffi_main.gc(ffi_lib.delta_stepper_alloc(a, t[0], t[1]), + ffi_lib.free) + s.setup_itersolve(sk) # Find the point where an XY move could result in excessive # tower movement half_min_step_dist = min([s.step_dist for s in self.steppers]) * .5 @@ -154,58 +162,14 @@ class DeltaKinematics: def move(self, print_time, move): if self.need_motor_enable: self._check_motor_enable(print_time) - axes_d = move.axes_d - move_d = move.move_d - movexy_r = 1. - movez_r = 0. - inv_movexy_d = 1. / move_d - if not axes_d[0] and not axes_d[1]: - # Z only move - movez_r = axes_d[2] * inv_movexy_d - movexy_r = inv_movexy_d = 0. - elif axes_d[2]: - # XY+Z move - movexy_d = math.sqrt(axes_d[0]**2 + axes_d[1]**2) - movexy_r = movexy_d * inv_movexy_d - movez_r = axes_d[2] * inv_movexy_d - inv_movexy_d = 1. / movexy_d - - origx, origy, origz = move.start_pos[:3] - - accel = move.accel - cruise_v = move.cruise_v - accel_d = move.accel_r * move_d - cruise_d = move.cruise_r * move_d - decel_d = move.decel_r * move_d - - for i in StepList: - # Calculate a virtual tower along the line of movement at - # the point closest to this stepper's tower. - towerx_d = self.towers[i][0] - origx - towery_d = self.towers[i][1] - origy - vt_startxy_d = (towerx_d*axes_d[0] + towery_d*axes_d[1])*inv_movexy_d - tangentxy_d2 = towerx_d**2 + towery_d**2 - vt_startxy_d**2 - vt_arm_d = math.sqrt(self.arm2[i] - tangentxy_d2) - vt_startz = origz - - # Generate steps - step_delta = self.steppers[i].step_delta - move_time = print_time - if accel_d: - step_delta(move_time, accel_d, move.start_v, accel, - vt_startz, vt_startxy_d, vt_arm_d, movez_r) - vt_startz += accel_d * movez_r - vt_startxy_d -= accel_d * movexy_r - move_time += move.accel_t - if cruise_d: - step_delta(move_time, cruise_d, cruise_v, 0., - vt_startz, vt_startxy_d, vt_arm_d, movez_r) - vt_startz += cruise_d * movez_r - vt_startxy_d -= cruise_d * movexy_r - move_time += move.cruise_t - if decel_d: - step_delta(move_time, decel_d, cruise_v, -accel, - vt_startz, vt_startxy_d, vt_arm_d, movez_r) + self.move_fill( + self.cmove, print_time, + move.accel_t, move.cruise_t, move.decel_t, + move.start_pos[0], move.start_pos[1], move.start_pos[2], + move.axes_d[0], move.axes_d[1], move.axes_d[2], + move.start_v, move.cruise_v, move.accel) + for stepper in self.steppers: + stepper.step_itersolve(self.cmove) # Helper functions for DELTA_CALIBRATE script def get_stable_position(self): return [int((ep - s.mcu_stepper.get_commanded_position()) diff --git a/klippy/mcu.py b/klippy/mcu.py index 4a34499a..d1f38c0e 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -26,7 +26,8 @@ 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._stepcompress_push_delta = None + self._stepcompress_push_const = self._itersolve_gen_steps = None + self._stepper_kinematics = None self.set_ignore_move(False) def get_mcu(self): return self._mcu @@ -40,6 +41,10 @@ class MCU_stepper: def setup_step_distance(self, step_dist): self._step_dist = step_dist self._inv_step_dist = 1. / step_dist + def setup_itersolve(self, sk): + self._stepper_kinematics = sk + self._ffi_lib.itersolve_set_stepcompress( + sk, self._stepqueue, self._step_dist) def build_config(self): max_error = self._mcu.get_max_stepper_error() min_stop_interval = max(0., self._min_stop_interval - max_error) @@ -67,6 +72,9 @@ class MCU_stepper: def get_step_dist(self): return self._step_dist def set_position(self, pos): + if self._stepper_kinematics is not None: + self._ffi_lib.itersolve_set_commanded_pos( + self._stepper_kinematics, pos) steppos = pos * self._inv_step_dist self._mcu_position_offset += self._commanded_pos - steppos self._commanded_pos = steppos @@ -82,10 +90,10 @@ class MCU_stepper: is not self._ffi_lib.stepcompress_push_const) if ignore_move: self._stepcompress_push_const = (lambda *args: 0) - self._stepcompress_push_delta = (lambda *args: 0) + self._itersolve_gen_steps = (lambda *args: 0) else: self._stepcompress_push_const = self._ffi_lib.stepcompress_push_const - self._stepcompress_push_delta = self._ffi_lib.stepcompress_push_delta + self._itersolve_gen_steps = self._ffi_lib.itersolve_gen_steps return was_ignore def note_homing_start(self, homing_clock): ret = self._ffi_lib.stepcompress_set_homing( @@ -127,14 +135,8 @@ class MCU_stepper: if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") self._commanded_pos += count - def step_delta(self, print_time, dist, start_v, accel - , height_base, startxy_d, arm_d, movez_r): - inv_step_dist = self._inv_step_dist - height = self._commanded_pos - height_base * inv_step_dist - count = self._stepcompress_push_delta( - self._stepqueue, print_time, dist * inv_step_dist, - start_v * inv_step_dist, accel * inv_step_dist, - height, startxy_d * inv_step_dist, arm_d * inv_step_dist, movez_r) + def step_itersolve(self, cmove): + count = self._itersolve_gen_steps(self._stepper_kinematics, cmove) if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") self._commanded_pos += count diff --git a/klippy/stepper.py b/klippy/stepper.py index 09000a42..825f636e 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -49,7 +49,8 @@ class PrinterStepper: 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_delta = self.mcu_stepper.step_delta + 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)) # Register STEPPER_BUZZ command stepper_buzz = printer.try_load_module(config, 'stepper_buzz') |