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
|
# Support for eddy current based Z probes
#
# Copyright (C) 2021-2024 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import logging, math, bisect
import mcu
from . import ldc1612, probe, manual_probe
# Tool for calibrating the sensor Z detection and applying that calibration
class EddyCalibration:
def __init__(self, config):
self.printer = config.get_printer()
self.name = config.get_name()
# Current calibration data
self.cal_freqs = []
self.cal_zpos = []
cal = config.get('calibrate', None)
if cal is not None:
cal = [list(map(float, d.strip().split(':', 1)))
for d in cal.split(',')]
self.load_calibration(cal)
# Probe calibrate state
self.probe_speed = 0.
# Register commands
cname = self.name.split()[-1]
gcode = self.printer.lookup_object('gcode')
gcode.register_mux_command("PROBE_EDDY_CURRENT_CALIBRATE", "CHIP",
cname, self.cmd_EDDY_CALIBRATE,
desc=self.cmd_EDDY_CALIBRATE_help)
def load_calibration(self, cal):
cal = sorted([(c[1], c[0]) for c in cal])
self.cal_freqs = [c[0] for c in cal]
self.cal_zpos = [c[1] for c in cal]
def apply_calibration(self, samples):
for i, (samp_time, freq, dummy_z) in enumerate(samples):
pos = bisect.bisect(self.cal_freqs, freq)
if pos >= len(self.cal_zpos):
zpos = -99.9
elif pos == 0:
zpos = 99.9
else:
# XXX - optimize and avoid div by zero
this_freq = self.cal_freqs[pos]
prev_freq = self.cal_freqs[pos - 1]
this_zpos = self.cal_zpos[pos]
prev_zpos = self.cal_zpos[pos - 1]
gain = (this_zpos - prev_zpos) / (this_freq - prev_freq)
offset = prev_zpos - prev_freq * gain
zpos = freq * gain + offset
samples[i] = (samp_time, freq, round(zpos, 6))
def do_calibration_moves(self, move_speed):
toolhead = self.printer.lookup_object('toolhead')
kin = toolhead.get_kinematics()
move = toolhead.manual_move
# Start data collection
msgs = []
is_finished = False
def handle_batch(msg):
if is_finished:
return False
msgs.append(msg)
return True
self.printer.lookup_object(self.name).add_client(handle_batch)
toolhead.dwell(1.)
# Move to each 50um position
max_z = 4
samp_dist = 0.050
num_steps = int(max_z / samp_dist + .5) + 1
start_pos = toolhead.get_position()
times = []
for i in range(num_steps):
# Move to next position (always descending to reduce backlash)
hop_pos = list(start_pos)
hop_pos[2] += i * samp_dist + 0.500
move(hop_pos, move_speed)
next_pos = list(start_pos)
next_pos[2] += i * samp_dist
move(next_pos, move_speed)
# Note sample timing
start_query_time = toolhead.get_last_move_time() + 0.050
end_query_time = start_query_time + 0.100
toolhead.dwell(0.200)
# Find Z position based on actual commanded stepper position
toolhead.flush_step_generation()
kin_spos = {s.get_name(): s.get_commanded_position()
for s in kin.get_steppers()}
kin_pos = kin.calc_position(kin_spos)
times.append((start_query_time, end_query_time, kin_pos[2]))
toolhead.dwell(1.0)
toolhead.wait_moves()
# Finish data collection
is_finished = True
# Correlate query responses
cal = {}
step = 0
for msg in msgs:
for query_time, freq, old_z in msg['data']:
# Add to step tracking
while step < len(times) and query_time > times[step][1]:
step += 1
if step < len(times) and query_time >= times[step][0]:
cal.setdefault(times[step][2], []).append(freq)
if len(cal) != len(times):
raise self.printer.command_error(
"Failed calibration - incomplete sensor data")
return cal
def calc_freqs(self, meas):
total_count = total_variance = 0
positions = {}
for pos, freqs in meas.items():
count = len(freqs)
freq_avg = float(sum(freqs)) / count
positions[pos] = freq_avg
total_count += count
total_variance += sum([(f - freq_avg)**2 for f in freqs])
return positions, math.sqrt(total_variance / total_count), total_count
def post_manual_probe(self, kin_pos):
if kin_pos is None:
# Manual Probe was aborted
return
curpos = list(kin_pos)
move = self.printer.lookup_object('toolhead').manual_move
# Move away from the bed
probe_calibrate_z = curpos[2]
curpos[2] += 5.
move(curpos, self.probe_speed)
# Move sensor over nozzle position
pprobe = self.printer.lookup_object("probe")
x_offset, y_offset, z_offset = pprobe.get_offsets()
curpos[0] -= x_offset
curpos[1] -= y_offset
move(curpos, self.probe_speed)
# Descend back to bed
curpos[2] -= 5. - 0.050
move(curpos, self.probe_speed)
# Perform calibration movement and capture
cal = self.do_calibration_moves(self.probe_speed)
# Calculate each sample position average and variance
positions, std, total = self.calc_freqs(cal)
last_freq = 0.
for pos, freq in reversed(sorted(positions.items())):
if freq <= last_freq:
raise self.printer.command_error(
"Failed calibration - frequency not increasing each step")
last_freq = freq
gcode = self.printer.lookup_object("gcode")
gcode.respond_info(
"probe_eddy_current: stddev=%.3f in %d queries\n"
"The SAVE_CONFIG command will update the printer config file\n"
"and restart the printer." % (std, total))
# Save results
cal_contents = []
for i, (pos, freq) in enumerate(sorted(positions.items())):
if not i % 3:
cal_contents.append('\n')
cal_contents.append("%.6f:%.3f" % (pos - probe_calibrate_z, freq))
cal_contents.append(',')
cal_contents.pop()
configfile = self.printer.lookup_object('configfile')
configfile.set(self.name, 'calibrate', ''.join(cal_contents))
cmd_EDDY_CALIBRATE_help = "Calibrate eddy current probe"
def cmd_EDDY_CALIBRATE(self, gcmd):
self.probe_speed = gcmd.get_float("PROBE_SPEED", 5., above=0.)
# Start manual probe
manual_probe.ManualProbeHelper(self.printer, gcmd,
self.post_manual_probe)
# Main "printer object"
class PrinterEddyProbe:
def __init__(self, config):
self.printer = config.get_printer()
self.calibration = EddyCalibration(config)
# Sensor type
sensors = { "ldc1612": ldc1612.LDC1612 }
sensor_type = config.getchoice('sensor_type', {s: s for s in sensors})
self.sensor_helper = sensors[sensor_type](config, self.calibration)
def add_client(self, cb):
self.sensor_helper.add_client(cb)
def load_config_prefix(config):
return PrinterEddyProbe(config)
|
