1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
|
# 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, probe_pos=False):
# 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
movepos = self._fill_coord(movepos)
error = None
try:
self.toolhead.move(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 probe_pos:
self.set_homed_position(
list(self.toolhead.get_kinematics().get_position()) + [None])
else:
self.toolhead.set_position(movepos)
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]))
|
