122 lines
5.0 KiB
Python
122 lines
5.0 KiB
Python
# Code for state tracking during homing operations
|
|
#
|
|
# Copyright (C) 2016,2017 Kevin O'Connor <kevin@koconnor.net>
|
|
#
|
|
# This file may be distributed under the terms of the GNU GPLv3 license.
|
|
import logging, math
|
|
|
|
HOMING_STEP_DELAY = 0.00000025
|
|
ENDSTOP_SAMPLE_TIME = .000015
|
|
ENDSTOP_SAMPLE_COUNT = 4
|
|
|
|
class Homing:
|
|
def __init__(self, toolhead):
|
|
self.toolhead = toolhead
|
|
self.changed_axes = []
|
|
self.verify_retract = True
|
|
def set_no_verify_retract(self):
|
|
self.verify_retract = False
|
|
def set_axes(self, axes):
|
|
self.changed_axes = axes
|
|
def get_axes(self):
|
|
return self.changed_axes
|
|
def _fill_coord(self, coord):
|
|
# Fill in any None entries in 'coord' with current toolhead position
|
|
thcoord = list(self.toolhead.get_position())
|
|
for i in range(len(coord)):
|
|
if coord[i] is not None:
|
|
thcoord[i] = coord[i]
|
|
return thcoord
|
|
def retract(self, newpos, speed):
|
|
self.toolhead.move(self._fill_coord(newpos), speed)
|
|
def set_homed_position(self, pos):
|
|
self.toolhead.set_position(self._fill_coord(pos))
|
|
def _get_homing_speed(self, speed, endstops):
|
|
# Round the requested homing speed so that it is an even
|
|
# number of ticks per step.
|
|
speed = min(speed, self.toolhead.get_max_velocity()[0])
|
|
mcu_stepper = endstops[0][0].get_steppers()[0]
|
|
adjusted_freq = mcu_stepper.get_mcu().get_adjusted_freq()
|
|
dist_ticks = adjusted_freq * mcu_stepper.get_step_dist()
|
|
ticks_per_step = math.ceil(dist_ticks / speed)
|
|
return dist_ticks / ticks_per_step
|
|
def homing_move(self, movepos, endstops, speed):
|
|
# Start endstop checking
|
|
print_time = self.toolhead.get_last_move_time()
|
|
for mcu_endstop, name in endstops:
|
|
min_step_dist = min([s.get_step_dist()
|
|
for s in mcu_endstop.get_steppers()])
|
|
mcu_endstop.home_start(
|
|
print_time, ENDSTOP_SAMPLE_TIME, ENDSTOP_SAMPLE_COUNT,
|
|
min_step_dist / speed)
|
|
# Issue move
|
|
error = None
|
|
try:
|
|
self.toolhead.move(self._fill_coord(movepos), speed)
|
|
except EndstopError as e:
|
|
error = "Error during homing move: %s" % (str(e),)
|
|
# Wait for endstops to trigger
|
|
move_end_print_time = self.toolhead.get_last_move_time()
|
|
self.toolhead.reset_print_time(print_time)
|
|
for mcu_endstop, name in endstops:
|
|
try:
|
|
mcu_endstop.home_wait(move_end_print_time)
|
|
except mcu_endstop.TimeoutError as e:
|
|
if error is None:
|
|
error = "Failed to home %s: %s" % (name, str(e))
|
|
if error is not None:
|
|
raise EndstopError(error)
|
|
def home(self, forcepos, movepos, endstops, speed, second_home=False):
|
|
# Alter kinematics class to think printer is at forcepos
|
|
self.toolhead.set_position(self._fill_coord(forcepos))
|
|
# Add a CPU delay when homing a large axis
|
|
if not second_home:
|
|
est_move_d = sum([abs(forcepos[i]-movepos[i])
|
|
for i in range(3) if movepos[i] is not None])
|
|
est_steps = sum([est_move_d / s.get_step_dist()
|
|
for es, n in endstops for s in es.get_steppers()])
|
|
self.toolhead.dwell(est_steps * HOMING_STEP_DELAY, check_stall=False)
|
|
speed = self._get_homing_speed(speed, endstops)
|
|
# Setup for retract verification
|
|
self.toolhead.get_last_move_time()
|
|
start_mcu_pos = [(s, name, s.get_mcu_position())
|
|
for es, name in endstops for s in es.get_steppers()]
|
|
# Issue homing move
|
|
self.homing_move(movepos, endstops, speed)
|
|
# Verify retract led to some movement on second home
|
|
if second_home and self.verify_retract:
|
|
for s, name, pos in start_mcu_pos:
|
|
if s.get_mcu_position() == pos:
|
|
raise EndstopError(
|
|
"Endstop %s still triggered after retract" % (name,))
|
|
def home_axes(self, axes):
|
|
self.changed_axes = axes
|
|
try:
|
|
self.toolhead.get_kinematics().home(self)
|
|
except EndstopError:
|
|
self.toolhead.motor_off()
|
|
raise
|
|
|
|
def query_endstops(toolhead):
|
|
print_time = toolhead.get_last_move_time()
|
|
steppers = toolhead.get_kinematics().get_steppers()
|
|
out = []
|
|
for s in steppers:
|
|
for mcu_endstop, name in s.get_endstops():
|
|
mcu_endstop.query_endstop(print_time)
|
|
for s in steppers:
|
|
for mcu_endstop, name in s.get_endstops():
|
|
out.append((name, mcu_endstop.query_endstop_wait()))
|
|
return out
|
|
|
|
def query_position(toolhead):
|
|
steppers = toolhead.get_kinematics().get_steppers()
|
|
return [(s.name.upper(), s.mcu_stepper.get_mcu_position()) for s in steppers]
|
|
|
|
class EndstopError(Exception):
|
|
pass
|
|
|
|
def EndstopMoveError(pos, msg="Move out of range"):
|
|
return EndstopError("%s: %.3f %.3f %.3f [%.3f]" % (
|
|
msg, pos[0], pos[1], pos[2], pos[3]))
|