From 890298d34d76923e895ff7905b9dbe374035e694 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Fri, 22 Jun 2018 13:03:07 -0400 Subject: [PATCH] itersolve: Support setting the stepper position via a cartesian coordinate Add support for an itersolve_set_position() function that sets a stepper position from a cartesian coordinate. This eliminates the need for both the python and C code to be able to translate from a cartesian coordinate to a stepper position. Signed-off-by: Kevin O'Connor --- docs/Code_Overview.md | 16 ++++++---------- klippy/cartesian.py | 5 ++--- klippy/chelper/__init__.py | 2 ++ klippy/chelper/itersolve.c | 10 ++++++++++ klippy/chelper/itersolve.h | 3 +++ klippy/corexy.py | 6 ++---- klippy/delta.py | 9 ++------- klippy/mcu.py | 8 +++++--- klippy/stepper.py | 4 ++-- 9 files changed, 34 insertions(+), 29 deletions(-) diff --git a/docs/Code_Overview.md b/docs/Code_Overview.md index 1bb883f4..64d54c28 100644 --- a/docs/Code_Overview.md +++ b/docs/Code_Overview.md @@ -319,19 +319,15 @@ Useful steps: seconds) to a cartesian coordinate (in millimeters), and then calculate the desired stepper position (in millimeters) from that cartesian coordinate. -4. Implement the `set_position()` method in the python code. This also - calculates the desired stepper positions given a cartesian - coordinate. -5. Implement the `calc_position()` method in the new kinematics class. +4. Implement the `calc_position()` method in the new kinematics class. This method is the inverse of set_position(). It does not need to be efficient as it is typically only called during homing and probing operations. -6. Implement the `move()` method. This method generally invokes the - iterative solver for each stepper. -7. Other methods. The `home()`, `check_move()`, and other methods - should also be implemented. However, at the start of development - one can use empty code here. -8. Implement test cases. Create a g-code file with a series of moves +5. Other methods. The `move()`, `home()`, `check_move()`, and other + methods should also be implemented. These functions are typically + used to provide kinematic specific checks. However, at the start of + development one can use boiler-plate code here. +6. Implement test cases. Create a g-code file with a series of moves that can test important cases for the given kinematics. Follow the [debugging documentation](Debugging.md) to convert this g-code file to micro-controller commands. This is useful to exercise corner diff --git a/klippy/cartesian.py b/klippy/cartesian.py index 7fc1419e..de6eb1df 100644 --- a/klippy/cartesian.py +++ b/klippy/cartesian.py @@ -54,9 +54,8 @@ class CartKinematics: def calc_position(self): return [rail.get_commanded_position() for rail in self.rails] def set_position(self, newpos, homing_axes): - for i in StepList: - rail = self.rails[i] - rail.set_position(newpos[i]) + for i, rail in enumerate(self.rails): + rail.set_position(newpos) if i in homing_axes: self.limits[i] = rail.get_range() def _home_axis(self, homing_state, axis, rail): diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 027cf277..083fe91b 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -51,6 +51,8 @@ defs_itersolve = """ int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m); void itersolve_set_stepcompress(struct stepper_kinematics *sk , struct stepcompress *sc, double step_dist); + void itersolve_set_position(struct stepper_kinematics *sk + , double x, double y, double z); void itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos); double itersolve_get_commanded_pos(struct stepper_kinematics *sk); """ diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c index c9ce9b8f..2bd27ac6 100644 --- a/klippy/chelper/itersolve.c +++ b/klippy/chelper/itersolve.c @@ -215,6 +215,16 @@ itersolve_set_stepcompress(struct stepper_kinematics *sk sk->step_dist = step_dist; } +void __visible +itersolve_set_position(struct stepper_kinematics *sk + , double x, double y, double z) +{ + struct move m; + memset(&m, 0, sizeof(m)); + move_fill(&m, 0., 0., 1., 0., x, y, z, 0., 1., 0., 0., 1., 0.); + sk->commanded_pos = sk->calc_position(sk, &m, 0.); +} + void __visible itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos) { diff --git a/klippy/chelper/itersolve.h b/klippy/chelper/itersolve.h index 23b3201d..25f26451 100644 --- a/klippy/chelper/itersolve.h +++ b/klippy/chelper/itersolve.h @@ -41,6 +41,9 @@ struct stepper_kinematics { int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m); void itersolve_set_stepcompress(struct stepper_kinematics *sk , struct stepcompress *sc, double step_dist); +void itersolve_set_position(struct stepper_kinematics *sk + , double x, double y, double z); void itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos); +double itersolve_get_commanded_pos(struct stepper_kinematics *sk); #endif // itersolve.h diff --git a/klippy/corexy.py b/klippy/corexy.py index 773b46d6..511e8fe3 100644 --- a/klippy/corexy.py +++ b/klippy/corexy.py @@ -46,10 +46,8 @@ class CoreXYKinematics: pos = [rail.get_commanded_position() for rail in self.rails] return [0.5 * (pos[0] + pos[1]), 0.5 * (pos[0] - pos[1]), pos[2]] def set_position(self, newpos, homing_axes): - pos = (newpos[0] + newpos[1], newpos[0] - newpos[1], newpos[2]) - for i in StepList: - rail = self.rails[i] - rail.set_position(pos[i]) + for i, rail in enumerate(self.rails): + rail.set_position(newpos) if i in homing_axes: self.limits[i] = rail.get_range() def home(self, homing_state): diff --git a/klippy/delta.py b/klippy/delta.py index efcfe7a5..d80959b5 100644 --- a/klippy/delta.py +++ b/klippy/delta.py @@ -87,19 +87,14 @@ class DeltaKinematics: self.set_position([0., 0., 0.], ()) def get_rails(self, flags=""): return list(self.rails) - def _cartesian_to_actuator(self, coord): - return [math.sqrt(self.arm2[i] - (self.towers[i][0] - coord[0])**2 - - (self.towers[i][1] - coord[1])**2) + coord[2] - for i in StepList] def _actuator_to_cartesian(self, pos): return actuator_to_cartesian(self.towers, self.arm2, pos) def calc_position(self): spos = [rail.get_commanded_position() for rail in self.rails] return self._actuator_to_cartesian(spos) def set_position(self, newpos, homing_axes): - pos = self._cartesian_to_actuator(newpos) - for i in StepList: - self.rails[i].set_position(pos[i]) + for rail in self.rails: + rail.set_position(newpos) self.limit_xy2 = -1. if tuple(homing_axes) == StepList: self.need_home = False diff --git a/klippy/mcu.py b/klippy/mcu.py index 1969ff82..8d7e86cb 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -71,9 +71,11 @@ class MCU_stepper: return self._oid def get_step_dist(self): return self._step_dist - def set_position(self, pos): - self._mcu_position_offset += self.get_commanded_position() - pos - self._ffi_lib.itersolve_set_commanded_pos(self._stepper_kinematics, pos) + def set_position(self, newpos): + orig_cmd_pos = self.get_commanded_position() + self._ffi_lib.itersolve_set_position( + self._stepper_kinematics, newpos[0], newpos[1], newpos[2]) + self._mcu_position_offset += orig_cmd_pos - self.get_commanded_position() def get_commanded_position(self): return self._ffi_lib.itersolve_get_commanded_pos( self._stepper_kinematics) diff --git a/klippy/stepper.py b/klippy/stepper.py index 5a9bee70..e68512f7 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -243,9 +243,9 @@ class PrinterRail: def set_max_jerk(self, max_halt_velocity, max_accel): for stepper in self.steppers: stepper.set_max_jerk(max_halt_velocity, max_accel) - def set_position(self, pos): + def set_position(self, newpos): for stepper in self.steppers: - stepper.set_position(pos) + stepper.set_position(newpos) def motor_enable(self, print_time, enable=0): for stepper in self.steppers: stepper.motor_enable(print_time, enable)