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
|
# 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)
for mcu_endstop, name in endstops:
mcu_endstop.home_finalize()
if error is not None:
raise EndstopError(error)
def homing_move(self, movepos, endstops, speed, probe_pos=False):
for mcu_endstop, name in endstops:
mcu_endstop.home_prepare()
self._homing_move(movepos, endstops, speed, probe_pos)
def home(self, forcepos, movepos, endstops, speed, second_home=False):
if second_home and forcepos == movepos:
return
# Alter kinematics class to think printer is at forcepos
homing_axes = [axis for axis in range(3) if forcepos[axis] is not None]
self.toolhead.set_position(
self._fill_coord(forcepos), homing_axes=homing_axes)
# Notify endstops of upcoming home
for mcu_endstop, name in endstops:
mcu_endstop.home_prepare()
# 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()
rails = toolhead.get_kinematics().get_rails()
out = []
for rail in rails:
for mcu_endstop, name in rail.get_endstops():
mcu_endstop.query_endstop(print_time)
for rail in rails:
for mcu_endstop, name in rail.get_endstops():
out.append((name, mcu_endstop.query_endstop_wait()))
return out
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]))
|
