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
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
|
# Axis Twist Compensation
#
# Copyright (C) 2022 Jeremy Tan <jeremytkw98@gmail.com>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import math
from . import manual_probe as ManualProbe, bed_mesh as BedMesh
DEFAULT_SAMPLE_COUNT = 3
DEFAULT_SPEED = 50.
DEFAULT_HORIZONTAL_MOVE_Z = 5.
class AxisTwistCompensation:
def __init__(self, config):
# get printer
self.printer = config.get_printer()
self.gcode = self.printer.lookup_object('gcode')
# get values from [axis_twist_compensation] section in printer .cfg
self.horizontal_move_z = config.getfloat('horizontal_move_z',
DEFAULT_HORIZONTAL_MOVE_Z)
self.speed = config.getfloat('speed', DEFAULT_SPEED)
self.calibrate_start_x = config.getfloat('calibrate_start_x')
self.calibrate_end_x = config.getfloat('calibrate_end_x')
self.calibrate_y = config.getfloat('calibrate_y')
self.z_compensations = config.getlists('z_compensations',
default=[], parser=float)
self.compensation_start_x = config.getfloat('compensation_start_x',
default=None)
self.compensation_end_x = config.getfloat('compensation_start_y',
default=None)
self.m = None
self.b = None
# setup calibrater
self.calibrater = Calibrater(self, config)
def get_z_compensation_value(self, pos):
if not self.z_compensations:
return 0
x_coord = pos[0]
z_compensations = self.z_compensations
sample_count = len(z_compensations)
spacing = ((self.calibrate_end_x - self.calibrate_start_x)
/ (sample_count - 1))
interpolate_t = (x_coord - self.calibrate_start_x) / spacing
interpolate_i = int(math.floor(interpolate_t))
interpolate_i = BedMesh.constrain(interpolate_i, 0, sample_count - 2)
interpolate_t -= interpolate_i
interpolated_z_compensation = BedMesh.lerp(
interpolate_t, z_compensations[interpolate_i],
z_compensations[interpolate_i + 1])
return interpolated_z_compensation
def clear_compensations(self):
self.z_compensations = []
self.m = None
self.b = None
class Calibrater:
def __init__(self, compensation, config):
# setup self attributes
self.compensation = compensation
self.printer = compensation.printer
self.gcode = self.printer.lookup_object('gcode')
self.probe = None
# probe settings are set to none, until they are available
self.lift_speed, self.probe_x_offset, self.probe_y_offset, _ = \
None, None, None, None
self.printer.register_event_handler("klippy:connect",
self._handle_connect)
self.speed = compensation.speed
self.horizontal_move_z = compensation.horizontal_move_z
self.start_point = (compensation.calibrate_start_x,
compensation.calibrate_y)
self.end_point = (compensation.calibrate_end_x,
compensation.calibrate_y)
self.results = None
self.current_point_index = None
self.gcmd = None
self.configname = config.get_name()
# register gcode handlers
self._register_gcode_handlers()
def _handle_connect(self):
self.probe = self.printer.lookup_object('probe', None)
if (self.probe is None):
config = self.printer.lookup_object('configfile')
raise config.error(
"AXIS_TWIST_COMPENSATION requires [probe] to be defined")
self.lift_speed = self.probe.get_lift_speed()
self.probe_x_offset, self.probe_y_offset, _ = \
self.probe.get_offsets()
def _register_gcode_handlers(self):
# register gcode handlers
self.gcode = self.printer.lookup_object('gcode')
self.gcode.register_command(
'AXIS_TWIST_COMPENSATION_CALIBRATE',
self.cmd_AXIS_TWIST_COMPENSATION_CALIBRATE,
desc=self.cmd_AXIS_TWIST_COMPENSATION_CALIBRATE_help)
cmd_AXIS_TWIST_COMPENSATION_CALIBRATE_help = """
Performs the x twist calibration wizard
Measure z probe offset at n points along the x axis,
and calculate x twist compensation
"""
def cmd_AXIS_TWIST_COMPENSATION_CALIBRATE(self, gcmd):
self.gcmd = gcmd
sample_count = gcmd.get_int('SAMPLE_COUNT', DEFAULT_SAMPLE_COUNT)
# check for valid sample_count
if sample_count is None or sample_count < 2:
raise self.gcmd.error(
"SAMPLE_COUNT to probe must be at least 2")
# clear the current config
self.compensation.clear_compensations()
# calculate some values
x_range = self.end_point[0] - self.start_point[0]
interval_dist = x_range / (sample_count - 1)
nozzle_points = self._calculate_nozzle_points(sample_count,
interval_dist)
probe_points = self._calculate_probe_points(
nozzle_points, self.probe_x_offset, self.probe_y_offset)
# verify no other manual probe is in progress
ManualProbe.verify_no_manual_probe(self.printer)
# begin calibration
self.current_point_index = 0
self.results = []
self._calibration(probe_points, nozzle_points, interval_dist)
def _calculate_nozzle_points(self, sample_count, interval_dist):
# calculate the points to put the probe at, returned as a list of tuples
nozzle_points = []
for i in range(sample_count):
x = self.start_point[0] + i * interval_dist
y = self.start_point[1]
nozzle_points.append((x, y))
return nozzle_points
def _calculate_probe_points(self, nozzle_points,
probe_x_offset, probe_y_offset):
# calculate the points to put the nozzle at
# returned as a list of tuples
probe_points = []
for point in nozzle_points:
x = point[0] - probe_x_offset
y = point[1] - probe_y_offset
probe_points.append((x, y))
return probe_points
def _move_helper(self, target_coordinates, override_speed=None):
# pad target coordinates
target_coordinates = \
(target_coordinates[0], target_coordinates[1], None) \
if len(target_coordinates) == 2 else target_coordinates
toolhead = self.printer.lookup_object('toolhead')
speed = self.speed if target_coordinates[2] == None else self.lift_speed
speed = override_speed if override_speed is not None else speed
toolhead.manual_move(target_coordinates, speed)
def _calibration(self, probe_points, nozzle_points, interval):
# begin the calibration process
self.gcmd.respond_info("AXIS_TWIST_COMPENSATION_CALIBRATE: "
"Probing point %d of %d" % (
self.current_point_index + 1,
len(probe_points)))
# horizontal_move_z (to prevent probe trigger or hitting bed)
self._move_helper((None, None, self.horizontal_move_z))
# move to point to probe
self._move_helper((probe_points[self.current_point_index][0],
probe_points[self.current_point_index][1], None))
# probe the point
self.current_measured_z = self.probe.run_probe(self.gcmd)[2]
# horizontal_move_z (to prevent probe trigger or hitting bed)
self._move_helper((None, None, self.horizontal_move_z))
# move the nozzle over the probe point
self._move_helper((nozzle_points[self.current_point_index]))
# start the manual (nozzle) probe
ManualProbe.ManualProbeHelper(
self.printer, self.gcmd,
self._manual_probe_callback_factory(
probe_points, nozzle_points, interval))
def _manual_probe_callback_factory(self, probe_points,
nozzle_points, interval):
# returns a callback function for the manual probe
is_end = self.current_point_index == len(probe_points) - 1
def callback(kin_pos):
if kin_pos is None:
# probe was cancelled
self.gcmd.respond_info(
"AXIS_TWIST_COMPENSATION_CALIBRATE: Probe cancelled, "
"calibration aborted")
return
z_offset = self.current_measured_z - kin_pos[2]
self.results.append(z_offset)
if is_end:
# end of calibration
self._finalize_calibration()
else:
# move to next point
self.current_point_index += 1
self._calibration(probe_points, nozzle_points, interval)
return callback
def _finalize_calibration(self):
# finalize the calibration process
# calculate average of results
avg = sum(self.results) / len(self.results)
# subtract average from each result
# so that they are independent of z_offset
self.results = [avg - x for x in self.results]
# save the config
configfile = self.printer.lookup_object('configfile')
values_as_str = ', '.join(["{:.6f}".format(x)
for x in self.results])
configfile.set(self.configname, 'z_compensations', values_as_str)
configfile.set(self.configname, 'compensation_start_x',
self.start_point[0])
configfile.set(self.configname, 'compensation_end_x',
self.end_point[0])
self.compensation.z_compensations = self.results
self.compensation.compensation_start_x = self.start_point[0]
self.compensation.compensation_end_x = self.end_point[0]
self.gcode.respond_info(
"AXIS_TWIST_COMPENSATION state has been saved "
"for the current session. The SAVE_CONFIG command will "
"update the printer config file and restart the printer.")
# output result
self.gcmd.respond_info(
"AXIS_TWIST_COMPENSATION_CALIBRATE: Calibration complete, "
"offsets: %s, mean z_offset: %f"
% (self.results, avg))
# klipper's entry point using [axis_twist_compensation] section in printer.cfg
def load_config(config):
return AxisTwistCompensation(config)
|
