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
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
|
# Helper code for implementing homing operations
#
# Copyright (C) 2016-2021 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import logging, math, collections
HOMING_START_DELAY = 0.001
ENDSTOP_SAMPLE_TIME = .000015
ENDSTOP_SAMPLE_COUNT = 4
# Return a completion that completes when all completions in a list complete
def multi_complete(printer, completions):
if len(completions) == 1:
return completions[0]
# Build completion that waits for all completions
reactor = printer.get_reactor()
cp = reactor.register_callback(lambda e: [c.wait() for c in completions])
# If any completion indicates an error, then exit main completion early
for c in completions:
reactor.register_callback(lambda e: cp.complete(1) if c.wait() else 0)
return cp
# Implementation of homing/probing moves
class HomingMove:
def __init__(self, printer, endstops, toolhead=None):
self.printer = printer
self.endstops = endstops
if toolhead is None:
toolhead = printer.lookup_object('toolhead')
self.toolhead = toolhead
self.end_mcu_pos = []
def get_mcu_endstops(self):
return [es for es, name in self.endstops]
def _calc_endstop_rate(self, mcu_endstop, movepos, speed):
startpos = self.toolhead.get_position()
axes_d = [mp - sp for mp, sp in zip(movepos, startpos)]
move_d = math.sqrt(sum([d*d for d in axes_d[:3]]))
move_t = move_d / speed
max_steps = max([(abs(s.calc_position_from_coord(startpos)
- s.calc_position_from_coord(movepos))
/ s.get_step_dist())
for s in mcu_endstop.get_steppers()])
if max_steps <= 0.:
return .001
return move_t / max_steps
def homing_move(self, movepos, speed, probe_pos=False,
triggered=True, check_triggered=True):
# Notify start of homing/probing move
self.printer.send_event("homing:homing_move_begin", self)
# Note start location
self.toolhead.flush_step_generation()
kin = self.toolhead.get_kinematics()
kin_spos = {s.get_name(): s.get_commanded_position()
for s in kin.get_steppers()}
start_mcu_pos = [(s, name, s.get_mcu_position())
for es, name in self.endstops
for s in es.get_steppers()]
# Start endstop checking
print_time = self.toolhead.get_last_move_time()
endstop_triggers = []
for mcu_endstop, name in self.endstops:
rest_time = self._calc_endstop_rate(mcu_endstop, movepos, speed)
wait = mcu_endstop.home_start(print_time, ENDSTOP_SAMPLE_TIME,
ENDSTOP_SAMPLE_COUNT, rest_time,
triggered=triggered)
endstop_triggers.append(wait)
all_endstop_trigger = multi_complete(self.printer, endstop_triggers)
self.toolhead.dwell(HOMING_START_DELAY)
# Issue move
error = None
try:
self.toolhead.drip_move(movepos, speed, all_endstop_trigger)
except self.printer.command_error 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()
for mcu_endstop, name in self.endstops:
trigger_time = mcu_endstop.home_wait(move_end_print_time)
if trigger_time < 0. and error is None:
error = "Communication timeout during homing %s" % (name,)
elif not trigger_time and check_triggered and error is None:
error = "No trigger on %s after full movement" % (name,)
# Determine stepper halt positions
self.toolhead.flush_step_generation()
self.end_mcu_pos = [(s, name, spos, s.get_mcu_position())
for s, name, spos in start_mcu_pos]
if probe_pos:
for s, name, spos, epos in self.end_mcu_pos:
sname = s.get_name()
if sname in kin_spos:
kin_spos[sname] += (epos - spos) * s.get_step_dist()
movepos = list(kin.calc_position(kin_spos))[:3] + movepos[3:]
self.toolhead.set_position(movepos)
# Signal homing/probing move complete
try:
self.printer.send_event("homing:homing_move_end", self)
except self.printer.command_error as e:
if error is None:
error = str(e)
if error is not None:
raise self.printer.command_error(error)
return movepos
def check_no_movement(self):
if self.printer.get_start_args().get('debuginput') is not None:
return None
for s, name, spos, epos in self.end_mcu_pos:
if spos == epos:
return name
return None
# State tracking of homing requests
class Homing:
def __init__(self, printer):
self.printer = printer
self.toolhead = printer.lookup_object('toolhead')
self.changed_axes = []
self.kin_spos = {}
def set_axes(self, axes):
self.changed_axes = axes
def get_axes(self):
return self.changed_axes
def get_stepper_trigger_positions(self):
return self.kin_spos
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 set_homed_position(self, pos):
self.toolhead.set_position(self._fill_coord(pos))
def home_rails(self, rails, forcepos, movepos):
# Notify of upcoming homing operation
self.printer.send_event("homing:home_rails_begin", self, rails)
# Alter kinematics class to think printer is at forcepos
homing_axes = [axis for axis in range(3) if forcepos[axis] is not None]
forcepos = self._fill_coord(forcepos)
movepos = self._fill_coord(movepos)
self.toolhead.set_position(forcepos, homing_axes=homing_axes)
# Perform first home
endstops = [es for rail in rails for es in rail.get_endstops()]
hi = rails[0].get_homing_info()
hmove = HomingMove(self.printer, endstops)
hmove.homing_move(movepos, hi.speed)
# Perform second home
if hi.retract_dist:
# Retract
axes_d = [mp - fp for mp, fp in zip(movepos, forcepos)]
move_d = math.sqrt(sum([d*d for d in axes_d[:3]]))
retract_r = min(1., hi.retract_dist / move_d)
retractpos = [mp - ad * retract_r
for mp, ad in zip(movepos, axes_d)]
self.toolhead.move(retractpos, hi.retract_speed)
# Home again
forcepos = [rp - ad * retract_r
for rp, ad in zip(retractpos, axes_d)]
self.toolhead.set_position(forcepos)
hmove = HomingMove(self.printer, endstops)
hmove.homing_move(movepos, hi.second_homing_speed)
if hmove.check_no_movement() is not None:
raise self.printer.command_error(
"Endstop %s still triggered after retract"
% (hmove.check_no_movement(),))
# Signal home operation complete
self.toolhead.flush_step_generation()
kin = self.toolhead.get_kinematics()
kin_spos = {s.get_name(): s.get_commanded_position()
for s in kin.get_steppers()}
self.kin_spos = dict(kin_spos)
self.printer.send_event("homing:home_rails_end", self, rails)
if kin_spos != self.kin_spos:
# Apply any homing offsets
adjustpos = kin.calc_position(self.kin_spos)
for axis in homing_axes:
movepos[axis] = adjustpos[axis]
self.toolhead.set_position(movepos)
class PrinterHoming:
def __init__(self, config):
self.printer = config.get_printer()
# Register g-code commands
gcode = self.printer.lookup_object('gcode')
gcode.register_command('G28', self.cmd_G28)
def manual_home(self, toolhead, endstops, pos, speed,
triggered, check_triggered):
hmove = HomingMove(self.printer, endstops, toolhead)
try:
hmove.homing_move(pos, speed, triggered=triggered,
check_triggered=check_triggered)
except self.printer.command_error:
if self.printer.is_shutdown():
raise self.printer.command_error(
"Homing failed due to printer shutdown")
raise
def probing_move(self, mcu_probe, pos, speed):
endstops = [(mcu_probe, "probe")]
hmove = HomingMove(self.printer, endstops)
try:
epos = hmove.homing_move(pos, speed, probe_pos=True)
except self.printer.command_error:
if self.printer.is_shutdown():
raise self.printer.command_error(
"Probing failed due to printer shutdown")
raise
if hmove.check_no_movement() is not None:
raise self.printer.command_error(
"Probe triggered prior to movement")
return epos
def cmd_G28(self, gcmd):
# Move to origin
axes = []
for pos, axis in enumerate('XYZ'):
if gcmd.get(axis, None) is not None:
axes.append(pos)
if not axes:
axes = [0, 1, 2]
homing_state = Homing(self.printer)
homing_state.set_axes(axes)
kin = self.printer.lookup_object('toolhead').get_kinematics()
try:
kin.home(homing_state)
except self.printer.command_error:
if self.printer.is_shutdown():
raise self.printer.command_error(
"Homing failed due to printer shutdown")
self.printer.lookup_object('stepper_enable').motor_off()
raise
def load_config(config):
return PrinterHoming(config)
|
