-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathknobprobe_old.py
208 lines (205 loc) · 9.87 KB
/
knobprobe_old.py
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
# Z-Probe support
#
# Copyright (C) 2017-2021 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import logging
import pins
from . import probe
from . import manual_probe
HINT_TIMEOUT = """
If the probe did not move far enough to trigger, then
consider reducing the Z axis minimum position so the probe
can travel further (the Z minimum position can be negative).
"""
class KnobProbe:
def __init__(self, config, mcu_probe):
self.printer = config.get_printer()
self.name = config.get_name()
self.mcu_probe = mcu_probe
self.speed = config.getfloat('speed', 5.0, above=0.)
self.lift_speed = config.getfloat('lift_speed', self.speed, above=0.)
self.x_offset = config.getfloat('x_offset', 0.)
self.y_offset = config.getfloat('y_offset', 0.)
self.z_offset = config.getfloat('z_offset')
self.probe_calibrate_z = 0.
self.multi_probe_pending = False
self.last_state = False
self.last_z_result = 0.
self.gcode_move = self.printer.load_object(config, "gcode_move")
# Infer Z position to move to during a probe
if config.has_section('stepper_z'):
zconfig = config.getsection('stepper_z')
self.z_position = zconfig.getfloat('position_min', 0.,
note_valid=False)
else:
pconfig = config.getsection('printer')
self.z_position = pconfig.getfloat('minimum_z_position', 0.,
note_valid=False)
# Multi-sample support (for improved accuracy)
self.sample_count = config.getint('samples', 1, minval=1)
self.sample_retract_dist = config.getfloat('sample_retract_dist', 2.,
above=0.)
atypes = {'median': 'median', 'average': 'average'}
self.samples_result = config.getchoice('samples_result', atypes,
'average')
self.samples_tolerance = config.getfloat('samples_tolerance', 0.100,
minval=0.)
self.samples_retries = config.getint('samples_tolerance_retries', 0,
minval=0)
# Register PROBE/QUERY_PROBE commands
self.gcode = self.printer.lookup_object('gcode')
self.gcode.register_command('KNOBPROBE', self.cmd_PROBE,
desc=self.cmd_PROBE_help)
self.gcode.register_command('QUERY_KNOBPROBE', self.cmd_QUERY_PROBE,
desc=self.cmd_QUERY_PROBE_help)
self.gcode.register_command('PROBE_KNOBACCURACY', self.cmd_PROBE_ACCURACY,
desc=self.cmd_PROBE_ACCURACY_help)
def multi_probe_begin(self):
self.mcu_probe.multi_probe_begin()
self.multi_probe_pending = True
def multi_probe_end(self):
if self.multi_probe_pending:
self.multi_probe_pending = False
self.mcu_probe.multi_probe_end()
def setup_pin(self, pin_type, pin_params):
if pin_type != 'endstop' or pin_params['pin'] != 'z_virtual_endstop':
raise pins.error("Probe virtual endstop only useful as endstop pin")
if pin_params['invert'] or pin_params['pullup']:
raise pins.error("Can not pullup/invert probe virtual endstop")
return self.mcu_probe
def get_lift_speed(self, gcmd=None):
if gcmd is not None:
return gcmd.get_float("LIFT_SPEED", self.lift_speed, above=0.)
return self.lift_speed
def get_offsets(self):
return self.x_offset, self.y_offset, self.z_offset
def _probe(self, speed):
toolhead = self.printer.lookup_object('toolhead')
curtime = self.printer.get_reactor().monotonic()
if 'z' not in toolhead.get_status(curtime)['homed_axes']:
raise self.printer.command_error("Must home before probe")
phoming = self.printer.lookup_object('homing')
pos = toolhead.get_position()
pos[2] = self.z_position
try:
epos = phoming.probing_move(self.mcu_probe, pos, speed)
except self.printer.command_error as e:
reason = str(e)
if "Timeout during endstop homing" in reason:
reason += HINT_TIMEOUT
raise self.printer.command_error(reason)
self.gcode.respond_info("KnobProbe triggered at z=%.6f"
% (epos[2]))
return epos[:3]
def _move(self, coord, speed):
self.printer.lookup_object('toolhead').manual_move(coord, speed)
def _calc_mean(self, positions):
count = float(len(positions))
return [sum([pos[i] for pos in positions]) / count
for i in range(3)]
def _calc_median(self, positions):
z_sorted = sorted(positions, key=(lambda p: p[2]))
middle = len(positions) // 2
if (len(positions) & 1) == 1:
# odd number of samples
return z_sorted[middle]
# even number of samples
return self._calc_mean(z_sorted[middle-1:middle+1])
def run_probe(self, gcmd):
speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.)
lift_speed = self.get_lift_speed(gcmd)
sample_count = gcmd.get_int("SAMPLES", self.sample_count, minval=1)
sample_retract_dist = gcmd.get_float("SAMPLE_RETRACT_DIST",
self.sample_retract_dist, above=0.)
samples_tolerance = gcmd.get_float("SAMPLES_TOLERANCE",
self.samples_tolerance, minval=0.)
samples_retries = gcmd.get_int("SAMPLES_TOLERANCE_RETRIES",
self.samples_retries, minval=0)
samples_result = gcmd.get("SAMPLES_RESULT", self.samples_result)
must_notify_multi_probe = not self.multi_probe_pending
if must_notify_multi_probe:
self.multi_probe_begin()
probexy = self.printer.lookup_object('toolhead').get_position()[:2]
retries = 0
positions = []
while len(positions) < sample_count:
# Probe position
pos = self._probe(speed)
positions.append(pos)
# Check samples tolerance
z_positions = [p[2] for p in positions]
if max(z_positions) - min(z_positions) > samples_tolerance:
if retries >= samples_retries:
raise gcmd.error("KnobProbe samples exceed samples_tolerance")
gcmd.respond_info("KnobProbe samples exceed tolerance. Retrying...")
retries += 1
positions = []
# Retract
if len(positions) < sample_count:
self._move(probexy + [pos[2] + sample_retract_dist], lift_speed)
if must_notify_multi_probe:
self.multi_probe_end()
# Calculate and return result
if samples_result == 'median':
return self._calc_median(positions)
return self._calc_mean(positions)
cmd_PROBE_help = "KnobProbe Z-height at current XY position"
def cmd_PROBE(self, gcmd):
pos = self.run_probe(gcmd)
gcmd.respond_info("Result is z=%.6f" % (pos[2],))
self.last_z_result = pos[2]
cmd_QUERY_PROBE_help = "Return the status of the Knobprobe"
def cmd_QUERY_PROBE(self, gcmd):
toolhead = self.printer.lookup_object('toolhead')
print_time = toolhead.get_last_move_time()
res = self.mcu_probe.query_endstop(print_time)
self.last_state = res
gcmd.respond_info("KnobProbe: %s" % (["open", "TRIGGERED"][not not res],))
def get_status(self, eventtime):
return {'last_query': self.last_state,
'last_z_result': self.last_z_result}
cmd_PROBE_ACCURACY_help = "KnobProbe Z-height accuracy at current XY position"
def cmd_PROBE_ACCURACY(self, gcmd):
speed = gcmd.get_float("PROBE_SPEED", self.speed, above=0.)
lift_speed = self.get_lift_speed(gcmd)
sample_count = gcmd.get_int("SAMPLES", 10, minval=1)
sample_retract_dist = gcmd.get_float("SAMPLE_RETRACT_DIST",
self.sample_retract_dist, above=0.)
toolhead = self.printer.lookup_object('toolhead')
pos = toolhead.get_position()
gcmd.respond_info("PROBE_ACCURACY at X:%.3f Y:%.3f Z:%.3f"
" (samples=%d retract=%.3f"
" speed=%.1f lift_speed=%.1f)\n"
% (pos[0], pos[1], pos[2],
sample_count, sample_retract_dist,
speed, lift_speed))
# Probe bed sample_count times
self.multi_probe_begin()
positions = []
while len(positions) < sample_count:
# Probe position
pos = self._probe(speed)
positions.append(pos)
# Retract
liftpos = [None, None, pos[2] + sample_retract_dist]
self._move(liftpos, lift_speed)
self.multi_probe_end()
# Calculate maximum, minimum and average values
max_value = max([p[2] for p in positions])
min_value = min([p[2] for p in positions])
range_value = max_value - min_value
avg_value = self._calc_mean(positions)[2]
median = self._calc_median(positions)[2]
# calculate the standard deviation
deviation_sum = 0
for i in range(len(positions)):
deviation_sum += pow(positions[i][2] - avg_value, 2.)
sigma = (deviation_sum / len(positions)) ** 0.5
# Show information
gcmd.respond_info(
"Knobprobe accuracy results: maximum %.6f, minimum %.6f, range %.6f, "
"average %.6f, median %.6f, standard deviation %.6f" % (
max_value, min_value, range_value, avg_value, median, sigma))
def load_config(config):
return KnobProbe(config, probe.ProbeEndstopWrapper(config))