stepper: Add get/set_tag_position() and convert calc_position()

Rename calc_position() to calc_tag_position() and have it calculate
the value of the position from the last stepper set_tag_position()
call.  This enables the calc_tag_position() code to be more flexible
as it can be run with arbitrary positions.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2019-11-13 17:59:40 -05:00
parent e0e2f15498
commit 224574da4a
11 changed files with 56 additions and 40 deletions

View File

@ -330,11 +330,12 @@ 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 `calc_position()` method in the new kinematics class.
This method calculates the position of the toolhead in cartesian
coordinates from the current position of each stepper. It does not
need to be efficient as it is typically only called during homing
and probing operations.
4. Implement the `calc_tag_position()` method in the new kinematics
class. This method calculates the position of the toolhead in
cartesian coordinates from the position of each stepper (as
returned by `stepper.get_tag_position()`). It does not need to be
efficient as it is typically only called during homing and probing
operations.
5. Other methods. Implement the `check_move()`, `get_status()`,
`get_steppers()`, `home()`, and `set_position()` methods. These
functions are typically used to provide kinematic specific

View File

@ -83,7 +83,10 @@ class ManualProbeHelper:
if toolhead_pos == self.last_toolhead_pos:
return self.last_kinematics_pos
self.toolhead.get_last_move_time()
kin_pos = self.toolhead.get_kinematics().calc_position()
kin = self.toolhead.get_kinematics()
for s in kin.get_steppers():
s.set_tag_position(s.get_commanded_position())
kin_pos = kin.calc_tag_position()
self.last_toolhead_pos = toolhead_pos
self.last_kinematics_pos = kin_pos
return kin_pos

View File

@ -690,11 +690,12 @@ class GCodeParser:
steppers = kin.get_steppers()
mcu_pos = " ".join(["%s:%d" % (s.get_name(), s.get_mcu_position())
for s in steppers])
stepper_pos = " ".join(
["%s:%.6f" % (s.get_name(), s.get_commanded_position())
for s in steppers])
kinematic_pos = " ".join(["%s:%.6f" % (a, v)
for a, v in zip("XYZE", kin.calc_position())])
for s in steppers:
s.set_tag_position(s.get_commanded_position())
stepper_pos = " ".join(["%s:%.6f" % (s.get_name(), s.get_tag_position())
for s in steppers])
kin_pos = " ".join(["%s:%.6f" % (a, v)
for a, v in zip("XYZ", kin.calc_tag_position())])
toolhead_pos = " ".join(["%s:%.6f" % (a, v) for a, v in zip(
"XYZE", self.toolhead.get_position())])
gcode_pos = " ".join(["%s:%.6f" % (a, v)
@ -703,16 +704,15 @@ class GCodeParser:
for a, v in zip("XYZE", self.base_position)])
homing_pos = " ".join(["%s:%.6f" % (a, v)
for a, v in zip("XYZ", self.homing_position)])
self.respond_info(
"mcu: %s\n"
"stepper: %s\n"
"kinematic: %s\n"
"toolhead: %s\n"
"gcode: %s\n"
"gcode base: %s\n"
"gcode homing: %s" % (
mcu_pos, stepper_pos, kinematic_pos, toolhead_pos,
gcode_pos, base_pos, homing_pos))
self.respond_info("mcu: %s\n"
"stepper: %s\n"
"kinematic: %s\n"
"toolhead: %s\n"
"gcode: %s\n"
"gcode base: %s\n"
"gcode homing: %s"
% (mcu_pos, stepper_pos, kin_pos, toolhead_pos,
gcode_pos, base_pos, homing_pos))
def request_restart(self, result):
if self.is_printer_ready:
print_time = self.toolhead.get_last_move_time()

View File

@ -67,8 +67,10 @@ class Homing:
if error is None:
error = "Failed to home %s: %s" % (name, str(e))
if probe_pos:
self.set_homed_position(
list(self.toolhead.get_kinematics().calc_position()) + [None])
kin = self.toolhead.get_kinematics()
for s in kin.get_steppers():
s.set_tag_position(s.get_commanded_position())
self.set_homed_position(kin.calc_tag_position())
else:
self.toolhead.set_position(movepos)
for mcu_endstop, name in endstops:
@ -116,7 +118,10 @@ class Homing:
ret = self.printer.send_event("homing:homed_rails", self, rails)
if any(ret):
# Apply any homing offsets
adjustpos = self.toolhead.get_kinematics().calc_position()
kin = self.toolhead.get_kinematics()
for s in kin.get_steppers():
s.set_tag_position(s.get_commanded_position())
adjustpos = kin.calc_tag_position()
for axis in homing_axes:
movepos[axis] = adjustpos[axis]
self.toolhead.set_position(movepos)

View File

@ -53,8 +53,8 @@ class CartKinematics:
if flags == "Z":
return self.rails[2].get_steppers()
return [s for rail in self.rails for s in rail.get_steppers()]
def calc_position(self):
return [rail.get_commanded_position() for rail in self.rails]
def calc_tag_position(self):
return [rail.get_tag_position() for rail in self.rails]
def set_position(self, newpos, homing_axes):
for i, rail in enumerate(self.rails):
rail.set_position(newpos)
@ -125,8 +125,9 @@ class CartKinematics:
self.rails[dc_axis].set_trapq(None)
dc_rail.set_trapq(toolhead.get_trapq())
self.rails[dc_axis] = dc_rail
extruder_pos = toolhead.get_position()[3]
toolhead.set_position(self.calc_position() + [extruder_pos])
pos = toolhead.get_position()
pos[dc_axis] = dc_rail.get_commanded_position()
toolhead.set_position(pos)
if self.limits[dc_axis][0] <= self.limits[dc_axis][1]:
self.limits[dc_axis] = dc_rail.get_range()
cmd_SET_DUAL_CARRIAGE_help = "Set which carriage is active"

View File

@ -43,8 +43,8 @@ class CoreXYKinematics:
if flags == "Z":
return self.rails[2].get_steppers()
return [s for rail in self.rails for s in rail.get_steppers()]
def calc_position(self):
pos = [rail.get_commanded_position() for rail in self.rails]
def calc_tag_position(self):
pos = [rail.get_tag_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):
for i, rail in enumerate(self.rails):

View File

@ -92,8 +92,8 @@ class DeltaKinematics:
def _actuator_to_cartesian(self, spos):
sphere_coords = [(t[0], t[1], sp) for t, sp in zip(self.towers, spos)]
return mathutil.trilateration(sphere_coords, self.arm2)
def calc_position(self):
spos = [rail.get_commanded_position() for rail in self.rails]
def calc_tag_position(self):
spos = [rail.get_tag_position() for rail in self.rails]
return self._actuator_to_cartesian(spos)
def set_position(self, newpos, homing_axes):
for rail in self.rails:

View File

@ -9,7 +9,7 @@ class NoneKinematics:
pass
def get_steppers(self, flags=""):
return []
def calc_position(self):
def calc_tag_position(self):
return [0, 0, 0]
def set_position(self, newpos, homing_axes):
pass

View File

@ -40,10 +40,10 @@ class PolarKinematics:
if flags == "Z":
return self.rails[1].get_steppers()
return list(self.steppers)
def calc_position(self):
bed_angle = self.steppers[0].get_commanded_position()
arm_pos = self.rails[0].get_commanded_position()
z_pos = self.rails[1].get_commanded_position()
def calc_tag_position(self):
bed_angle = self.steppers[0].get_tag_position()
arm_pos = self.rails[0].get_tag_position()
z_pos = self.rails[1].get_tag_position()
return [math.cos(bed_angle) * arm_pos, math.sin(bed_angle) * arm_pos,
z_pos]
def set_position(self, newpos, homing_axes):

View File

@ -31,9 +31,9 @@ class WinchKinematics:
self.set_position([0., 0., 0.], ())
def get_steppers(self, flags=""):
return list(self.steppers)
def calc_position(self):
def calc_tag_position(self):
# Use only first three steppers to calculate cartesian position
spos = [s.get_commanded_position() for s in self.steppers[:3]]
spos = [s.get_tag_position() for s in self.steppers[:3]]
return mathutil.trilateration(self.anchors[:3], [sp*sp for sp in spos])
def set_position(self, newpos, homing_axes):
for s in self.steppers:

View File

@ -29,7 +29,7 @@ class MCU_stepper:
"Stepper dir pin must be on same mcu as step pin")
self._dir_pin = dir_pin_params['pin']
self._invert_dir = dir_pin_params['invert']
self._mcu_position_offset = 0.
self._mcu_position_offset = self._tag_position = 0.
self._min_stop_interval = 0.
self._reset_cmd_id = self._get_position_cmd = None
self._active_callbacks = []
@ -107,6 +107,10 @@ class MCU_stepper:
if mcu_pos >= 0.:
return int(mcu_pos + 0.5)
return int(mcu_pos - 0.5)
def get_tag_position(self):
return self._tag_position
def set_tag_position(self, position):
self._tag_position = position
def set_stepper_kinematics(self, sk):
old_sk = self._stepper_kinematics
self._stepper_kinematics = sk
@ -193,6 +197,8 @@ class PrinterRail:
self.endstops = []
self.add_extra_stepper(config)
self.get_commanded_position = self.steppers[0].get_commanded_position
self.get_tag_position = self.steppers[0].get_tag_position
self.set_tag_position = self.steppers[0].set_tag_position
# Primary endstop position
mcu_endstop = self.endstops[0][0]
if hasattr(mcu_endstop, "get_position_endstop"):