|
|
@@ -26,12 +26,15 @@
|
|
26
|
26
|
|
|
27
|
27
|
#include "../gcode.h"
|
|
28
|
28
|
#include "../../module/delta.h"
|
|
29
|
|
-#include "../../module/probe.h"
|
|
30
|
29
|
#include "../../module/motion.h"
|
|
31
|
30
|
#include "../../module/stepper.h"
|
|
32
|
31
|
#include "../../module/endstops.h"
|
|
33
|
32
|
#include "../../lcd/ultralcd.h"
|
|
34
|
33
|
|
|
|
34
|
+#if HAS_BED_PROBE
|
|
|
35
|
+ #include "../../module/probe.h"
|
|
|
36
|
+#endif
|
|
|
37
|
+
|
|
35
|
38
|
#if HOTENDS > 1
|
|
36
|
39
|
#include "../../module/tool_change.h"
|
|
37
|
40
|
#endif
|
|
|
@@ -43,7 +46,7 @@
|
|
43
|
46
|
constexpr uint8_t _7P_STEP = 1, // 7-point step - to change number of calibration points
|
|
44
|
47
|
_4P_STEP = _7P_STEP * 2, // 4-point step
|
|
45
|
48
|
NPP = _7P_STEP * 6; // number of calibration points on the radius
|
|
46
|
|
-enum CalEnum : char { // the 7 main calibration points - add definitions if needed
|
|
|
49
|
+enum CalEnum : char { // the 7 main calibration points - add definitions if needed
|
|
47
|
50
|
CEN = 0,
|
|
48
|
51
|
__A = 1,
|
|
49
|
52
|
_AB = __A + _7P_STEP,
|
|
|
@@ -60,7 +63,54 @@ enum CalEnum : char { // the 7 main calibration po
|
|
60
|
63
|
#define LOOP_CAL_RAD(VAR) LOOP_CAL_PT(VAR, __A, _7P_STEP)
|
|
61
|
64
|
#define LOOP_CAL_ACT(VAR, _4P, _OP) LOOP_CAL_PT(VAR, _OP ? _AB : __A, _4P ? _4P_STEP : _7P_STEP)
|
|
62
|
65
|
|
|
63
|
|
-static void print_signed_float(const char * const prefix, const float &f) {
|
|
|
66
|
+#if HOTENDS > 1
|
|
|
67
|
+ const uint8_t old_tool_index = active_extruder;
|
|
|
68
|
+ #define AC_CLEANUP() ac_cleanup(old_tool_index)
|
|
|
69
|
+#else
|
|
|
70
|
+ #define AC_CLEANUP() ac_cleanup()
|
|
|
71
|
+#endif
|
|
|
72
|
+
|
|
|
73
|
+float lcd_probe_pt(const float &rx, const float &ry);
|
|
|
74
|
+
|
|
|
75
|
+bool ac_home() {
|
|
|
76
|
+ endstops.enable(true);
|
|
|
77
|
+ if (!home_delta())
|
|
|
78
|
+ return false;
|
|
|
79
|
+ endstops.not_homing();
|
|
|
80
|
+ return true;
|
|
|
81
|
+}
|
|
|
82
|
+
|
|
|
83
|
+void ac_setup(const bool reset_bed) {
|
|
|
84
|
+ #if HOTENDS > 1
|
|
|
85
|
+ tool_change(0, 0, true);
|
|
|
86
|
+ #endif
|
|
|
87
|
+
|
|
|
88
|
+ stepper.synchronize();
|
|
|
89
|
+ setup_for_endstop_or_probe_move();
|
|
|
90
|
+
|
|
|
91
|
+ #if HAS_LEVELING
|
|
|
92
|
+ if (reset_bed) reset_bed_level(); // After full calibration bed-level data is no longer valid
|
|
|
93
|
+ #endif
|
|
|
94
|
+}
|
|
|
95
|
+
|
|
|
96
|
+void ac_cleanup(
|
|
|
97
|
+ #if HOTENDS > 1
|
|
|
98
|
+ const uint8_t old_tool_index
|
|
|
99
|
+ #endif
|
|
|
100
|
+) {
|
|
|
101
|
+ #if ENABLED(DELTA_HOME_TO_SAFE_ZONE)
|
|
|
102
|
+ do_blocking_move_to_z(delta_clip_start_height);
|
|
|
103
|
+ #endif
|
|
|
104
|
+ #if HAS_BED_PROBE
|
|
|
105
|
+ STOW_PROBE();
|
|
|
106
|
+ #endif
|
|
|
107
|
+ clean_up_after_endstop_or_probe_move();
|
|
|
108
|
+ #if HOTENDS > 1
|
|
|
109
|
+ tool_change(old_tool_index, 0, true);
|
|
|
110
|
+ #endif
|
|
|
111
|
+}
|
|
|
112
|
+
|
|
|
113
|
+void print_signed_float(const char * const prefix, const float &f) {
|
|
64
|
114
|
SERIAL_PROTOCOLPGM(" ");
|
|
65
|
115
|
serialprintPGM(prefix);
|
|
66
|
116
|
SERIAL_PROTOCOLCHAR(':');
|
|
|
@@ -68,7 +118,10 @@ static void print_signed_float(const char * const prefix, const float &f) {
|
|
68
|
118
|
SERIAL_PROTOCOL_F(f, 2);
|
|
69
|
119
|
}
|
|
70
|
120
|
|
|
71
|
|
-static void print_G33_settings(const bool end_stops, const bool tower_angles) {
|
|
|
121
|
+/**
|
|
|
122
|
+ * - Print the delta settings
|
|
|
123
|
+ */
|
|
|
124
|
+static void print_calibration_settings(const bool end_stops, const bool tower_angles) {
|
|
72
|
125
|
SERIAL_PROTOCOLPAIR(".Height:", delta_height);
|
|
73
|
126
|
if (end_stops) {
|
|
74
|
127
|
print_signed_float(PSTR("Ex"), delta_endstop_adj[A_AXIS]);
|
|
|
@@ -89,16 +142,25 @@ static void print_G33_settings(const bool end_stops, const bool tower_angles) {
|
|
89
|
142
|
if ((!end_stops && tower_angles) || (end_stops && !tower_angles)) { // XOR
|
|
90
|
143
|
SERIAL_PROTOCOLPAIR(" Radius:", delta_radius);
|
|
91
|
144
|
}
|
|
|
145
|
+ #if HAS_BED_PROBE
|
|
|
146
|
+ if (!end_stops && !tower_angles) {
|
|
|
147
|
+ SERIAL_PROTOCOL_SP(30);
|
|
|
148
|
+ print_signed_float(PSTR("Offset"), zprobe_zoffset);
|
|
|
149
|
+ }
|
|
|
150
|
+ #endif
|
|
92
|
151
|
SERIAL_EOL();
|
|
93
|
152
|
}
|
|
94
|
153
|
|
|
95
|
|
-static void print_G33_results(const float z_at_pt[NPP + 1], const bool tower_points, const bool opposite_points) {
|
|
|
154
|
+/**
|
|
|
155
|
+ * - Print the probe results
|
|
|
156
|
+ */
|
|
|
157
|
+static void print_calibration_results(const float z_pt[NPP + 1], const bool tower_points, const bool opposite_points) {
|
|
96
|
158
|
SERIAL_PROTOCOLPGM(". ");
|
|
97
|
|
- print_signed_float(PSTR("c"), z_at_pt[CEN]);
|
|
|
159
|
+ print_signed_float(PSTR("c"), z_pt[CEN]);
|
|
98
|
160
|
if (tower_points) {
|
|
99
|
|
- print_signed_float(PSTR(" x"), z_at_pt[__A]);
|
|
100
|
|
- print_signed_float(PSTR(" y"), z_at_pt[__B]);
|
|
101
|
|
- print_signed_float(PSTR(" z"), z_at_pt[__C]);
|
|
|
161
|
+ print_signed_float(PSTR(" x"), z_pt[__A]);
|
|
|
162
|
+ print_signed_float(PSTR(" y"), z_pt[__B]);
|
|
|
163
|
+ print_signed_float(PSTR(" z"), z_pt[__C]);
|
|
102
|
164
|
}
|
|
103
|
165
|
if (tower_points && opposite_points) {
|
|
104
|
166
|
SERIAL_EOL();
|
|
|
@@ -106,50 +168,63 @@ static void print_G33_results(const float z_at_pt[NPP + 1], const bool tower_poi
|
|
106
|
168
|
SERIAL_PROTOCOL_SP(13);
|
|
107
|
169
|
}
|
|
108
|
170
|
if (opposite_points) {
|
|
109
|
|
- print_signed_float(PSTR("yz"), z_at_pt[_BC]);
|
|
110
|
|
- print_signed_float(PSTR("zx"), z_at_pt[_CA]);
|
|
111
|
|
- print_signed_float(PSTR("xy"), z_at_pt[_AB]);
|
|
|
171
|
+ print_signed_float(PSTR("yz"), z_pt[_BC]);
|
|
|
172
|
+ print_signed_float(PSTR("zx"), z_pt[_CA]);
|
|
|
173
|
+ print_signed_float(PSTR("xy"), z_pt[_AB]);
|
|
112
|
174
|
}
|
|
113
|
175
|
SERIAL_EOL();
|
|
114
|
176
|
}
|
|
115
|
177
|
|
|
116
|
178
|
/**
|
|
117
|
|
- * After G33:
|
|
118
|
|
- * - Move to the print ceiling (DELTA_HOME_TO_SAFE_ZONE only)
|
|
119
|
|
- * - Stow the probe
|
|
120
|
|
- * - Restore endstops state
|
|
121
|
|
- * - Select the old tool, if needed
|
|
|
179
|
+ * - Calculate the standard deviation from the zero plane
|
|
122
|
180
|
*/
|
|
123
|
|
-static void G33_cleanup(
|
|
124
|
|
- #if HOTENDS > 1
|
|
125
|
|
- const uint8_t old_tool_index
|
|
126
|
|
- #endif
|
|
127
|
|
-) {
|
|
128
|
|
- #if ENABLED(DELTA_HOME_TO_SAFE_ZONE)
|
|
129
|
|
- do_blocking_move_to_z(delta_clip_start_height);
|
|
130
|
|
- #endif
|
|
131
|
|
- STOW_PROBE();
|
|
132
|
|
- clean_up_after_endstop_or_probe_move();
|
|
133
|
|
- #if HOTENDS > 1
|
|
134
|
|
- tool_change(old_tool_index, 0, true);
|
|
135
|
|
- #endif
|
|
|
181
|
+static float std_dev_points(float z_pt[NPP + 1], const bool _0p_cal, const bool _1p_cal, const bool _4p_cal, const bool _4p_opp) {
|
|
|
182
|
+ if (!_0p_cal) {
|
|
|
183
|
+ float S2 = sq(z_pt[CEN]);
|
|
|
184
|
+ int16_t N = 1;
|
|
|
185
|
+ if (!_1p_cal) { // std dev from zero plane
|
|
|
186
|
+ LOOP_CAL_ACT(rad, _4p_cal, _4p_opp) {
|
|
|
187
|
+ S2 += sq(z_pt[rad]);
|
|
|
188
|
+ N++;
|
|
|
189
|
+ }
|
|
|
190
|
+ return round(SQRT(S2 / N) * 1000.0) / 1000.0 + 0.00001;
|
|
|
191
|
+ }
|
|
|
192
|
+ }
|
|
|
193
|
+ return 0.00001;
|
|
136
|
194
|
}
|
|
137
|
195
|
|
|
138
|
|
-inline float calibration_probe(const float nx, const float ny, const bool stow) {
|
|
|
196
|
+/**
|
|
|
197
|
+ * - Probe a point
|
|
|
198
|
+ */
|
|
|
199
|
+static float calibration_probe(const float &nx, const float &ny, const bool stow, const bool set_up) {
|
|
139
|
200
|
#if HAS_BED_PROBE
|
|
140
|
|
- return probe_pt(nx, ny, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, false);
|
|
|
201
|
+ return probe_pt(nx, ny, set_up ? PROBE_PT_BIG_RAISE : stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, false);
|
|
141
|
202
|
#else
|
|
142
|
203
|
UNUSED(stow);
|
|
|
204
|
+ UNUSED(set_up);
|
|
143
|
205
|
return lcd_probe_pt(nx, ny);
|
|
144
|
206
|
#endif
|
|
145
|
207
|
}
|
|
146
|
208
|
|
|
147
|
|
-static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points, const bool towers_set, const bool stow_after_each) {
|
|
|
209
|
+#if HAS_BED_PROBE
|
|
|
210
|
+ static float probe_z_shift(const float center) {
|
|
|
211
|
+ STOW_PROBE();
|
|
|
212
|
+ endstops.enable_z_probe(false);
|
|
|
213
|
+ float z_shift = lcd_probe_pt(0, 0) - center;
|
|
|
214
|
+ endstops.enable_z_probe(true);
|
|
|
215
|
+ return z_shift;
|
|
|
216
|
+ }
|
|
|
217
|
+#endif
|
|
|
218
|
+
|
|
|
219
|
+/**
|
|
|
220
|
+ * - Probe a grid
|
|
|
221
|
+ */
|
|
|
222
|
+static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_points, const bool towers_set, const bool stow_after_each, const bool set_up) {
|
|
148
|
223
|
const bool _0p_calibration = probe_points == 0,
|
|
149
|
|
- _1p_calibration = probe_points == 1,
|
|
|
224
|
+ _1p_calibration = probe_points == 1 || probe_points == -1,
|
|
150
|
225
|
_4p_calibration = probe_points == 2,
|
|
151
|
226
|
_4p_opposite_points = _4p_calibration && !towers_set,
|
|
152
|
|
- _7p_calibration = probe_points >= 3 || probe_points == 0,
|
|
|
227
|
+ _7p_calibration = probe_points >= 3,
|
|
153
|
228
|
_7p_no_intermediates = probe_points == 3,
|
|
154
|
229
|
_7p_1_intermediates = probe_points == 4,
|
|
155
|
230
|
_7p_2_intermediates = probe_points == 5,
|
|
|
@@ -159,28 +234,28 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points,
|
|
159
|
234
|
_7p_11_intermediates = probe_points == 9,
|
|
160
|
235
|
_7p_14_intermediates = probe_points == 10,
|
|
161
|
236
|
_7p_intermed_points = probe_points >= 4,
|
|
162
|
|
- _7p_6_centre = probe_points >= 5 && probe_points <= 7,
|
|
163
|
|
- _7p_9_centre = probe_points >= 8;
|
|
|
237
|
+ _7p_6_center = probe_points >= 5 && probe_points <= 7,
|
|
|
238
|
+ _7p_9_center = probe_points >= 8;
|
|
164
|
239
|
|
|
165
|
|
- LOOP_CAL_ALL(axis) z_at_pt[axis] = 0.0;
|
|
|
240
|
+ LOOP_CAL_ALL(rad) z_pt[rad] = 0.0;
|
|
166
|
241
|
|
|
167
|
242
|
if (!_0p_calibration) {
|
|
168
|
243
|
|
|
169
|
244
|
if (!_7p_no_intermediates && !_7p_4_intermediates && !_7p_11_intermediates) { // probe the center
|
|
170
|
|
- z_at_pt[CEN] += calibration_probe(0, 0, stow_after_each);
|
|
171
|
|
- if (isnan(z_at_pt[CEN])) return NAN;
|
|
|
245
|
+ z_pt[CEN] += calibration_probe(0, 0, stow_after_each, set_up);
|
|
|
246
|
+ if (isnan(z_pt[CEN])) return false;
|
|
172
|
247
|
}
|
|
173
|
248
|
|
|
174
|
249
|
if (_7p_calibration) { // probe extra center points
|
|
175
|
|
- const float start = _7p_9_centre ? _CA + _7P_STEP / 3.0 : _7p_6_centre ? _CA : __C,
|
|
176
|
|
- steps = _7p_9_centre ? _4P_STEP / 3.0 : _7p_6_centre ? _7P_STEP : _4P_STEP;
|
|
177
|
|
- I_LOOP_CAL_PT(axis, start, steps) {
|
|
178
|
|
- const float a = RADIANS(210 + (360 / NPP) * (axis - 1)),
|
|
|
250
|
+ const float start = _7p_9_center ? _CA + _7P_STEP / 3.0 : _7p_6_center ? _CA : __C,
|
|
|
251
|
+ steps = _7p_9_center ? _4P_STEP / 3.0 : _7p_6_center ? _7P_STEP : _4P_STEP;
|
|
|
252
|
+ I_LOOP_CAL_PT(rad, start, steps) {
|
|
|
253
|
+ const float a = RADIANS(210 + (360 / NPP) * (rad - 1)),
|
|
179
|
254
|
r = delta_calibration_radius * 0.1;
|
|
180
|
|
- z_at_pt[CEN] += calibration_probe(cos(a) * r, sin(a) * r, stow_after_each);
|
|
181
|
|
- if (isnan(z_at_pt[CEN])) return NAN;
|
|
|
255
|
+ z_pt[CEN] += calibration_probe(cos(a) * r, sin(a) * r, stow_after_each, set_up);
|
|
|
256
|
+ if (isnan(z_pt[CEN])) return false;
|
|
182
|
257
|
}
|
|
183
|
|
- z_at_pt[CEN] /= float(_7p_2_intermediates ? 7 : probe_points);
|
|
|
258
|
+ z_pt[CEN] /= float(_7p_2_intermediates ? 7 : probe_points);
|
|
184
|
259
|
}
|
|
185
|
260
|
|
|
186
|
261
|
if (!_1p_calibration) { // probe the radius
|
|
|
@@ -195,182 +270,150 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points,
|
|
195
|
270
|
_7p_no_intermediates ? _7P_STEP : // 1r * 6 + 3c = 9
|
|
196
|
271
|
_4P_STEP; // .5r * 6 + 1c = 4
|
|
197
|
272
|
bool zig_zag = true;
|
|
198
|
|
- F_LOOP_CAL_PT(axis, start, _7p_9_centre ? steps * 3 : steps) {
|
|
199
|
|
- const int8_t offset = _7p_9_centre ? 1 : 0;
|
|
200
|
|
- for (int8_t circle = -offset; circle <= offset; circle++) {
|
|
201
|
|
- const float a = RADIANS(210 + (360 / NPP) * (axis - 1)),
|
|
202
|
|
- r = delta_calibration_radius * (1 + 0.1 * (zig_zag ? circle : - circle)),
|
|
203
|
|
- interpol = fmod(axis, 1);
|
|
204
|
|
- const float z_temp = calibration_probe(cos(a) * r, sin(a) * r, stow_after_each);
|
|
205
|
|
- if (isnan(z_temp)) return NAN;
|
|
|
273
|
+ F_LOOP_CAL_PT(rad, start, _7p_9_center ? steps * 3 : steps) {
|
|
|
274
|
+ const int8_t offset = _7p_9_center ? 2 : 0;
|
|
|
275
|
+ for (int8_t circle = 0; circle <= offset; circle++) {
|
|
|
276
|
+ const float a = RADIANS(210 + (360 / NPP) * (rad - 1)),
|
|
|
277
|
+ r = delta_calibration_radius * (1 - 0.1 * (zig_zag ? offset - circle : circle)),
|
|
|
278
|
+ interpol = fmod(rad, 1);
|
|
|
279
|
+ const float z_temp = calibration_probe(cos(a) * r, sin(a) * r, stow_after_each, set_up);
|
|
|
280
|
+ if (isnan(z_temp)) return false;
|
|
206
|
281
|
// split probe point to neighbouring calibration points
|
|
207
|
|
- z_at_pt[uint8_t(round(axis - interpol + NPP - 1)) % NPP + 1] += z_temp * sq(cos(RADIANS(interpol * 90)));
|
|
208
|
|
- z_at_pt[uint8_t(round(axis - interpol)) % NPP + 1] += z_temp * sq(sin(RADIANS(interpol * 90)));
|
|
|
282
|
+ z_pt[uint8_t(round(rad - interpol + NPP - 1)) % NPP + 1] += z_temp * sq(cos(RADIANS(interpol * 90)));
|
|
|
283
|
+ z_pt[uint8_t(round(rad - interpol)) % NPP + 1] += z_temp * sq(sin(RADIANS(interpol * 90)));
|
|
209
|
284
|
}
|
|
210
|
285
|
zig_zag = !zig_zag;
|
|
211
|
286
|
}
|
|
212
|
287
|
if (_7p_intermed_points)
|
|
213
|
|
- LOOP_CAL_RAD(axis)
|
|
214
|
|
- z_at_pt[axis] /= _7P_STEP / steps;
|
|
215
|
|
- }
|
|
|
288
|
+ LOOP_CAL_RAD(rad)
|
|
|
289
|
+ z_pt[rad] /= _7P_STEP / steps;
|
|
216
|
290
|
|
|
217
|
|
- float S1 = z_at_pt[CEN],
|
|
218
|
|
- S2 = sq(z_at_pt[CEN]);
|
|
219
|
|
- int16_t N = 1;
|
|
220
|
|
- if (!_1p_calibration) { // std dev from zero plane
|
|
221
|
|
- LOOP_CAL_ACT(axis, _4p_calibration, _4p_opposite_points) {
|
|
222
|
|
- S1 += z_at_pt[axis];
|
|
223
|
|
- S2 += sq(z_at_pt[axis]);
|
|
224
|
|
- N++;
|
|
225
|
|
- }
|
|
226
|
|
- return round(SQRT(S2 / N) * 1000.0) / 1000.0 + 0.00001;
|
|
|
291
|
+ do_blocking_move_to_xy(0.0, 0.0);
|
|
227
|
292
|
}
|
|
228
|
293
|
}
|
|
229
|
|
-
|
|
230
|
|
- return 0.00001;
|
|
|
294
|
+ return true;
|
|
231
|
295
|
}
|
|
232
|
296
|
|
|
233
|
|
-#if HAS_BED_PROBE
|
|
234
|
|
-
|
|
235
|
|
- static bool G33_auto_tune() {
|
|
236
|
|
- float z_at_pt[NPP + 1] = { 0.0 },
|
|
237
|
|
- z_at_pt_base[NPP + 1] = { 0.0 },
|
|
238
|
|
- z_temp, h_fac = 0.0, r_fac = 0.0, a_fac = 0.0, norm = 0.8;
|
|
239
|
|
-
|
|
240
|
|
- #define ZP(N,I) ((N) * z_at_pt[I])
|
|
241
|
|
- #define Z06(I) ZP(6, I)
|
|
242
|
|
- #define Z03(I) ZP(3, I)
|
|
243
|
|
- #define Z02(I) ZP(2, I)
|
|
244
|
|
- #define Z01(I) ZP(1, I)
|
|
245
|
|
- #define Z32(I) ZP(3/2, I)
|
|
246
|
|
-
|
|
247
|
|
- SERIAL_PROTOCOLPGM("AUTO TUNE baseline");
|
|
248
|
|
- SERIAL_EOL();
|
|
249
|
|
- if (isnan(probe_G33_points(z_at_pt_base, 3, true, false))) return false;
|
|
250
|
|
- print_G33_results(z_at_pt_base, true, true);
|
|
|
297
|
+/**
|
|
|
298
|
+ * kinematics routines and auto tune matrix scaling parameters:
|
|
|
299
|
+ * see https://github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for
|
|
|
300
|
+ * - formulae for approximative forward kinematics in the end-stop displacement matrix
|
|
|
301
|
+ * - definition of the matrix scaling parameters
|
|
|
302
|
+ */
|
|
|
303
|
+static void reverse_kinematics_probe_points(float z_pt[NPP + 1], float mm_at_pt_axis[NPP + 1][ABC]) {
|
|
|
304
|
+ float pos[XYZ] = { 0.0 };
|
|
|
305
|
+
|
|
|
306
|
+ LOOP_CAL_ALL(rad) {
|
|
|
307
|
+ const float a = RADIANS(210 + (360 / NPP) * (rad - 1)),
|
|
|
308
|
+ r = (rad == CEN ? 0.0 : delta_calibration_radius);
|
|
|
309
|
+ pos[X_AXIS] = cos(a) * r;
|
|
|
310
|
+ pos[Y_AXIS] = sin(a) * r;
|
|
|
311
|
+ pos[Z_AXIS] = z_pt[rad];
|
|
|
312
|
+ inverse_kinematics(pos);
|
|
|
313
|
+ LOOP_XYZ(axis) mm_at_pt_axis[rad][axis] = delta[axis];
|
|
|
314
|
+ }
|
|
|
315
|
+}
|
|
251
|
316
|
|
|
252
|
|
- LOOP_XYZ(axis) {
|
|
253
|
|
- delta_endstop_adj[axis] -= 1.0;
|
|
254
|
|
- recalc_delta_settings();
|
|
|
317
|
+static void forward_kinematics_probe_points(float mm_at_pt_axis[NPP + 1][ABC], float z_pt[NPP + 1]) {
|
|
|
318
|
+ const float r_quot = delta_calibration_radius / delta_radius;
|
|
|
319
|
+
|
|
|
320
|
+ #define ZPP(N,I,A) ((1 / 3.0 + r_quot * (N) / 3.0 ) * mm_at_pt_axis[I][A])
|
|
|
321
|
+ #define Z00(I, A) ZPP( 0, I, A)
|
|
|
322
|
+ #define Zp1(I, A) ZPP(+1, I, A)
|
|
|
323
|
+ #define Zm1(I, A) ZPP(-1, I, A)
|
|
|
324
|
+ #define Zp2(I, A) ZPP(+2, I, A)
|
|
|
325
|
+ #define Zm2(I, A) ZPP(-2, I, A)
|
|
|
326
|
+
|
|
|
327
|
+ z_pt[CEN] = Z00(CEN, A_AXIS) + Z00(CEN, B_AXIS) + Z00(CEN, C_AXIS);
|
|
|
328
|
+ z_pt[__A] = Zp2(__A, A_AXIS) + Zm1(__A, B_AXIS) + Zm1(__A, C_AXIS);
|
|
|
329
|
+ z_pt[__B] = Zm1(__B, A_AXIS) + Zp2(__B, B_AXIS) + Zm1(__B, C_AXIS);
|
|
|
330
|
+ z_pt[__C] = Zm1(__C, A_AXIS) + Zm1(__C, B_AXIS) + Zp2(__C, C_AXIS);
|
|
|
331
|
+ z_pt[_BC] = Zm2(_BC, A_AXIS) + Zp1(_BC, B_AXIS) + Zp1(_BC, C_AXIS);
|
|
|
332
|
+ z_pt[_CA] = Zp1(_CA, A_AXIS) + Zm2(_CA, B_AXIS) + Zp1(_CA, C_AXIS);
|
|
|
333
|
+ z_pt[_AB] = Zp1(_AB, A_AXIS) + Zp1(_AB, B_AXIS) + Zm2(_AB, C_AXIS);
|
|
|
334
|
+}
|
|
255
|
335
|
|
|
256
|
|
- endstops.enable(true);
|
|
257
|
|
- if (!home_delta()) return false;
|
|
258
|
|
- endstops.not_homing();
|
|
|
336
|
+static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], float delta_e[ABC], float delta_r, float delta_t[ABC]) {
|
|
|
337
|
+ const float z_center = z_pt[CEN];
|
|
|
338
|
+ float diff_mm_at_pt_axis[NPP + 1][ABC],
|
|
|
339
|
+ new_mm_at_pt_axis[NPP + 1][ABC];
|
|
259
|
340
|
|
|
260
|
|
- SERIAL_PROTOCOLPGM("Tuning E");
|
|
261
|
|
- SERIAL_CHAR(tolower(axis_codes[axis]));
|
|
262
|
|
- SERIAL_EOL();
|
|
|
341
|
+ reverse_kinematics_probe_points(z_pt, diff_mm_at_pt_axis);
|
|
263
|
342
|
|
|
264
|
|
- if (isnan(probe_G33_points(z_at_pt, 3, true, false))) return false;
|
|
265
|
|
- LOOP_CAL_ALL(axis) z_at_pt[axis] -= z_at_pt_base[axis];
|
|
266
|
|
- print_G33_results(z_at_pt, true, true);
|
|
267
|
|
- delta_endstop_adj[axis] += 1.0;
|
|
268
|
|
- recalc_delta_settings();
|
|
269
|
|
- switch (axis) {
|
|
270
|
|
- case A_AXIS :
|
|
271
|
|
- h_fac += 4.0 / (Z03(CEN) +Z01(__A) +Z32(_CA) +Z32(_AB)); // Offset by X-tower end-stop
|
|
272
|
|
- break;
|
|
273
|
|
- case B_AXIS :
|
|
274
|
|
- h_fac += 4.0 / (Z03(CEN) +Z01(__B) +Z32(_BC) +Z32(_AB)); // Offset by Y-tower end-stop
|
|
275
|
|
- break;
|
|
276
|
|
- case C_AXIS :
|
|
277
|
|
- h_fac += 4.0 / (Z03(CEN) +Z01(__C) +Z32(_BC) +Z32(_CA) ); // Offset by Z-tower end-stop
|
|
278
|
|
- break;
|
|
279
|
|
- }
|
|
280
|
|
- }
|
|
281
|
|
- h_fac /= 3.0;
|
|
282
|
|
- h_fac *= norm; // Normalize to 1.02 for Kossel mini
|
|
|
343
|
+ delta_radius += delta_r;
|
|
|
344
|
+ LOOP_XYZ(axis) delta_tower_angle_trim[axis] += delta_t[axis];
|
|
|
345
|
+ recalc_delta_settings();
|
|
|
346
|
+ reverse_kinematics_probe_points(z_pt, new_mm_at_pt_axis);
|
|
283
|
347
|
|
|
284
|
|
- for (int8_t zig_zag = -1; zig_zag < 2; zig_zag += 2) {
|
|
285
|
|
- delta_radius += 1.0 * zig_zag;
|
|
286
|
|
- recalc_delta_settings();
|
|
|
348
|
+ LOOP_XYZ(axis) LOOP_CAL_ALL(rad) diff_mm_at_pt_axis[rad][axis] -= new_mm_at_pt_axis[rad][axis] + delta_e[axis];
|
|
|
349
|
+ forward_kinematics_probe_points(diff_mm_at_pt_axis, z_pt);
|
|
287
|
350
|
|
|
288
|
|
- endstops.enable(true);
|
|
289
|
|
- if (!home_delta()) return false;
|
|
290
|
|
- endstops.not_homing();
|
|
|
351
|
+ LOOP_CAL_RAD(rad) z_pt[rad] -= z_pt[CEN] - z_center;
|
|
|
352
|
+ z_pt[CEN] = z_center;
|
|
291
|
353
|
|
|
292
|
|
- SERIAL_PROTOCOLPGM("Tuning R");
|
|
293
|
|
- SERIAL_PROTOCOL(zig_zag == -1 ? "-" : "+");
|
|
294
|
|
- SERIAL_EOL();
|
|
295
|
|
- if (isnan(probe_G33_points(z_at_pt, 3, true, false))) return false;
|
|
296
|
|
- LOOP_CAL_ALL(axis) z_at_pt[axis] -= z_at_pt_base[axis];
|
|
297
|
|
- print_G33_results(z_at_pt, true, true);
|
|
298
|
|
- delta_radius -= 1.0 * zig_zag;
|
|
299
|
|
- recalc_delta_settings();
|
|
300
|
|
- r_fac -= zig_zag * 6.0 / (Z03(__A) +Z03(__B) +Z03(__C) +Z03(_BC) +Z03(_CA) +Z03(_AB)); // Offset by delta radius
|
|
301
|
|
- }
|
|
302
|
|
- r_fac /= 2.0;
|
|
303
|
|
- r_fac *= 3 * norm; // Normalize to 2.25 for Kossel mini
|
|
304
|
|
-
|
|
305
|
|
- LOOP_XYZ(axis) {
|
|
306
|
|
- delta_tower_angle_trim[axis] += 1.0;
|
|
307
|
|
- delta_endstop_adj[(axis + 1) % 3] -= 1.0 / 4.5;
|
|
308
|
|
- delta_endstop_adj[(axis + 2) % 3] += 1.0 / 4.5;
|
|
309
|
|
- z_temp = MAX3(delta_endstop_adj[A_AXIS], delta_endstop_adj[B_AXIS], delta_endstop_adj[C_AXIS]);
|
|
310
|
|
- delta_height -= z_temp;
|
|
311
|
|
- LOOP_XYZ(axis) delta_endstop_adj[axis] -= z_temp;
|
|
312
|
|
- recalc_delta_settings();
|
|
|
354
|
+ delta_radius -= delta_r;
|
|
|
355
|
+ LOOP_XYZ(axis) delta_tower_angle_trim[axis] -= delta_t[axis];
|
|
|
356
|
+ recalc_delta_settings();
|
|
|
357
|
+}
|
|
313
|
358
|
|
|
314
|
|
- endstops.enable(true);
|
|
315
|
|
- if (!home_delta()) return false;
|
|
316
|
|
- endstops.not_homing();
|
|
|
359
|
+static float auto_tune_h() {
|
|
|
360
|
+ const float r_quot = delta_calibration_radius / delta_radius;
|
|
|
361
|
+ float h_fac = 0.0;
|
|
317
|
362
|
|
|
318
|
|
- SERIAL_PROTOCOLPGM("Tuning T");
|
|
319
|
|
- SERIAL_CHAR(tolower(axis_codes[axis]));
|
|
320
|
|
- SERIAL_EOL();
|
|
|
363
|
+ h_fac = r_quot / (2.0 / 3.0);
|
|
|
364
|
+ h_fac = 1.0 / h_fac; // (2/3)/CR
|
|
|
365
|
+ return h_fac;
|
|
|
366
|
+}
|
|
321
|
367
|
|
|
322
|
|
- if (isnan(probe_G33_points(z_at_pt, 3, true, false))) return false;
|
|
323
|
|
- LOOP_CAL_ALL(axis) z_at_pt[axis] -= z_at_pt_base[axis];
|
|
324
|
|
- print_G33_results(z_at_pt, true, true);
|
|
|
368
|
+static float auto_tune_r() {
|
|
|
369
|
+ const float diff = 0.01;
|
|
|
370
|
+ float r_fac = 0.0,
|
|
|
371
|
+ z_pt[NPP + 1] = { 0.0 },
|
|
|
372
|
+ delta_e[ABC] = {0.0},
|
|
|
373
|
+ delta_r = {0.0},
|
|
|
374
|
+ delta_t[ABC] = {0.0};
|
|
|
375
|
+
|
|
|
376
|
+ delta_r = diff;
|
|
|
377
|
+ calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
|
|
|
378
|
+ r_fac = -(z_pt[__A] + z_pt[__B] + z_pt[__C] + z_pt[_BC] + z_pt[_CA] + z_pt[_AB]) / 6.0;
|
|
|
379
|
+ r_fac = diff / r_fac / 3.0; // 1/(3*delta_Z)
|
|
|
380
|
+ return r_fac;
|
|
|
381
|
+}
|
|
325
|
382
|
|
|
326
|
|
- delta_tower_angle_trim[axis] -= 1.0;
|
|
327
|
|
- delta_endstop_adj[(axis+1) % 3] += 1.0/4.5;
|
|
328
|
|
- delta_endstop_adj[(axis+2) % 3] -= 1.0/4.5;
|
|
329
|
|
- z_temp = MAX3(delta_endstop_adj[A_AXIS], delta_endstop_adj[B_AXIS], delta_endstop_adj[C_AXIS]);
|
|
330
|
|
- delta_height -= z_temp;
|
|
331
|
|
- LOOP_XYZ(axis) delta_endstop_adj[axis] -= z_temp;
|
|
332
|
|
- recalc_delta_settings();
|
|
333
|
|
- switch (axis) {
|
|
334
|
|
- case A_AXIS :
|
|
335
|
|
- a_fac += 4.0 / ( Z06(__B) -Z06(__C) +Z06(_CA) -Z06(_AB)); // Offset by alpha tower angle
|
|
336
|
|
- break;
|
|
337
|
|
- case B_AXIS :
|
|
338
|
|
- a_fac += 4.0 / (-Z06(__A) +Z06(__C) -Z06(_BC) +Z06(_AB)); // Offset by beta tower angle
|
|
339
|
|
- break;
|
|
340
|
|
- case C_AXIS :
|
|
341
|
|
- a_fac += 4.0 / (Z06(__A) -Z06(__B) +Z06(_BC) -Z06(_CA) ); // Offset by gamma tower angle
|
|
342
|
|
- break;
|
|
343
|
|
- }
|
|
344
|
|
- }
|
|
345
|
|
- a_fac /= 3.0;
|
|
346
|
|
- a_fac *= norm; // Normalize to 0.83 for Kossel mini
|
|
347
|
|
-
|
|
348
|
|
- endstops.enable(true);
|
|
349
|
|
- if (!home_delta()) return false;
|
|
350
|
|
- endstops.not_homing();
|
|
351
|
|
- print_signed_float(PSTR( "H_FACTOR: "), h_fac);
|
|
352
|
|
- print_signed_float(PSTR(" R_FACTOR: "), r_fac);
|
|
353
|
|
- print_signed_float(PSTR(" A_FACTOR: "), a_fac);
|
|
354
|
|
- SERIAL_EOL();
|
|
355
|
|
- SERIAL_PROTOCOLPGM("Copy these values to Configuration.h");
|
|
356
|
|
- SERIAL_EOL();
|
|
357
|
|
- return true;
|
|
|
383
|
+static float auto_tune_a() {
|
|
|
384
|
+ const float diff = 0.01;
|
|
|
385
|
+ float a_fac = 0.0,
|
|
|
386
|
+ z_pt[NPP + 1] = { 0.0 },
|
|
|
387
|
+ delta_e[ABC] = {0.0},
|
|
|
388
|
+ delta_r = {0.0},
|
|
|
389
|
+ delta_t[ABC] = {0.0};
|
|
|
390
|
+
|
|
|
391
|
+ LOOP_XYZ(axis) {
|
|
|
392
|
+ LOOP_XYZ(axis_2) delta_t[axis_2] = 0.0;
|
|
|
393
|
+ delta_t[axis] = diff;
|
|
|
394
|
+ calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
|
|
|
395
|
+ a_fac += z_pt[uint8_t((axis * _4P_STEP) - _7P_STEP + NPP) % NPP + 1] / 6.0;
|
|
|
396
|
+ a_fac -= z_pt[uint8_t((axis * _4P_STEP) + 1 + _7P_STEP)] / 6.0;
|
|
358
|
397
|
}
|
|
359
|
|
-
|
|
360
|
|
-#endif // HAS_BED_PROBE
|
|
|
398
|
+ a_fac = diff / a_fac / 3.0; // 1/(3*delta_Z)
|
|
|
399
|
+ return a_fac;
|
|
|
400
|
+}
|
|
361
|
401
|
|
|
362
|
402
|
/**
|
|
363
|
403
|
* G33 - Delta '1-4-7-point' Auto-Calibration
|
|
364
|
|
- * Calibrate height, endstops, delta radius, and tower angles.
|
|
|
404
|
+ * Calibrate height, z_offset, endstops, delta radius, and tower angles.
|
|
365
|
405
|
*
|
|
366
|
406
|
* Parameters:
|
|
367
|
407
|
*
|
|
|
408
|
+ * S Setup mode; disables probe protection
|
|
|
409
|
+ *
|
|
368
|
410
|
* Pn Number of probe points:
|
|
369
|
|
- * P0 No probe. Normalize only.
|
|
370
|
|
- * P1 Probe center and set height only.
|
|
371
|
|
- * P2 Probe center and towers. Set height, endstops and delta radius.
|
|
372
|
|
- * P3 Probe all positions: center, towers and opposite towers. Set all.
|
|
373
|
|
- * P4-P10 Probe all positions + at different intermediate locations and average them.
|
|
|
411
|
+ * P-1 Checks the z_offset with a center probe and paper test.
|
|
|
412
|
+ * P0 Normalizes calibration.
|
|
|
413
|
+ * P1 Calibrates height only with center probe.
|
|
|
414
|
+ * P2 Probe center and towers. Calibrate height, endstops and delta radius.
|
|
|
415
|
+ * P3 Probe all positions: center, towers and opposite towers. Calibrate all.
|
|
|
416
|
+ * P4-P10 Probe all positions at different intermediate locations and average them.
|
|
374
|
417
|
*
|
|
375
|
418
|
* T Don't calibrate tower angle corrections
|
|
376
|
419
|
*
|
|
|
@@ -378,8 +421,6 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points,
|
|
378
|
421
|
*
|
|
379
|
422
|
* Fn Force to run at least n iterations and take the best result
|
|
380
|
423
|
*
|
|
381
|
|
- * A Auto-tune calibration factors (set in Configuration.h)
|
|
382
|
|
- *
|
|
383
|
424
|
* Vn Verbose level:
|
|
384
|
425
|
* V0 Dry-run mode. Report settings and probe results. No calibration.
|
|
385
|
426
|
* V1 Report start and end settings only
|
|
|
@@ -390,19 +431,22 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points,
|
|
390
|
431
|
*/
|
|
391
|
432
|
void GcodeSuite::G33() {
|
|
392
|
433
|
|
|
393
|
|
- const int8_t probe_points = parser.intval('P', DELTA_CALIBRATION_DEFAULT_POINTS);
|
|
394
|
|
- if (!WITHIN(probe_points, 0, 10)) {
|
|
395
|
|
- SERIAL_PROTOCOLLNPGM("?(P)oints is implausible (0-10).");
|
|
396
|
|
- return;
|
|
397
|
|
- }
|
|
|
434
|
+ const bool set_up =
|
|
|
435
|
+ #if HAS_BED_PROBE
|
|
|
436
|
+ parser.seen('S');
|
|
|
437
|
+ #else
|
|
|
438
|
+ false;
|
|
|
439
|
+ #endif
|
|
398
|
440
|
|
|
399
|
|
- const int8_t verbose_level = parser.byteval('V', 1);
|
|
400
|
|
- if (!WITHIN(verbose_level, 0, 3)) {
|
|
401
|
|
- SERIAL_PROTOCOLLNPGM("?(V)erbose level is implausible (0-3).");
|
|
|
441
|
+ const int8_t probe_points = set_up ? 2 : parser.intval('P', DELTA_CALIBRATION_DEFAULT_POINTS);
|
|
|
442
|
+ if (!WITHIN(probe_points, -1, 10)) {
|
|
|
443
|
+ SERIAL_PROTOCOLLNPGM("?(P)oints is implausible (-1 - 10).");
|
|
402
|
444
|
return;
|
|
403
|
445
|
}
|
|
404
|
446
|
|
|
405
|
|
- const float calibration_precision = parser.floatval('C', 0.0);
|
|
|
447
|
+ const bool towers_set = !parser.seen('T');
|
|
|
448
|
+
|
|
|
449
|
+ const float calibration_precision = set_up ? Z_CLEARANCE_BETWEEN_PROBES / 5.0 : parser.floatval('C', 0.0);
|
|
406
|
450
|
if (calibration_precision < 0) {
|
|
407
|
451
|
SERIAL_PROTOCOLLNPGM("?(C)alibration precision is implausible (>=0).");
|
|
408
|
452
|
return;
|
|
|
@@ -410,36 +454,52 @@ void GcodeSuite::G33() {
|
|
410
|
454
|
|
|
411
|
455
|
const int8_t force_iterations = parser.intval('F', 0);
|
|
412
|
456
|
if (!WITHIN(force_iterations, 0, 30)) {
|
|
413
|
|
- SERIAL_PROTOCOLLNPGM("?(F)orce iteration is implausible (0-30).");
|
|
|
457
|
+ SERIAL_PROTOCOLLNPGM("?(F)orce iteration is implausible (0 - 30).");
|
|
|
458
|
+ return;
|
|
|
459
|
+ }
|
|
|
460
|
+
|
|
|
461
|
+ const int8_t verbose_level = parser.byteval('V', 1);
|
|
|
462
|
+ if (!WITHIN(verbose_level, 0, 3)) {
|
|
|
463
|
+ SERIAL_PROTOCOLLNPGM("?(V)erbose level is implausible (0 - 3).");
|
|
414
|
464
|
return;
|
|
415
|
465
|
}
|
|
416
|
466
|
|
|
417
|
|
- const bool towers_set = !parser.boolval('T'),
|
|
418
|
|
- auto_tune = parser.boolval('A'),
|
|
419
|
|
- stow_after_each = parser.boolval('E'),
|
|
420
|
|
- _0p_calibration = probe_points == 0,
|
|
421
|
|
- _1p_calibration = probe_points == 1,
|
|
|
467
|
+ const bool stow_after_each = parser.seen('E');
|
|
|
468
|
+
|
|
|
469
|
+ if (set_up) {
|
|
|
470
|
+ delta_height = 999.99;
|
|
|
471
|
+ delta_radius = DELTA_PRINTABLE_RADIUS;
|
|
|
472
|
+ ZERO(delta_endstop_adj);
|
|
|
473
|
+ ZERO(delta_tower_angle_trim);
|
|
|
474
|
+ recalc_delta_settings();
|
|
|
475
|
+ }
|
|
|
476
|
+
|
|
|
477
|
+ const bool _0p_calibration = probe_points == 0,
|
|
|
478
|
+ _1p_calibration = probe_points == 1 || probe_points == -1,
|
|
422
|
479
|
_4p_calibration = probe_points == 2,
|
|
423
|
|
- _7p_9_centre = probe_points >= 8,
|
|
424
|
|
- _tower_results = (_4p_calibration && towers_set)
|
|
425
|
|
- || probe_points >= 3 || probe_points == 0,
|
|
426
|
|
- _opposite_results = (_4p_calibration && !towers_set)
|
|
427
|
|
- || probe_points >= 3 || probe_points == 0,
|
|
428
|
|
- _endstop_results = probe_points != 1,
|
|
429
|
|
- _angle_results = (probe_points >= 3 || probe_points == 0) && towers_set;
|
|
|
480
|
+ _4p_opposite_points = _4p_calibration && !towers_set,
|
|
|
481
|
+ _7p_9_center = probe_points >= 8,
|
|
|
482
|
+ _tower_results = (_4p_calibration && towers_set) || probe_points >= 3,
|
|
|
483
|
+ _opposite_results = (_4p_calibration && !towers_set) || probe_points >= 3,
|
|
|
484
|
+ _endstop_results = probe_points != 1 && probe_points != -1 && probe_points != 0,
|
|
|
485
|
+ _angle_results = probe_points >= 3 && towers_set;
|
|
430
|
486
|
const static char save_message[] PROGMEM = "Save with M500 and/or copy to Configuration.h";
|
|
431
|
487
|
int8_t iterations = 0;
|
|
432
|
488
|
float test_precision,
|
|
433
|
489
|
zero_std_dev = (verbose_level ? 999.0 : 0.0), // 0.0 in dry-run mode : forced end
|
|
434
|
490
|
zero_std_dev_min = zero_std_dev,
|
|
|
491
|
+ zero_std_dev_old = zero_std_dev,
|
|
|
492
|
+ h_factor,
|
|
|
493
|
+ r_factor,
|
|
|
494
|
+ a_factor,
|
|
435
|
495
|
e_old[ABC] = {
|
|
436
|
496
|
delta_endstop_adj[A_AXIS],
|
|
437
|
497
|
delta_endstop_adj[B_AXIS],
|
|
438
|
498
|
delta_endstop_adj[C_AXIS]
|
|
439
|
499
|
},
|
|
440
|
|
- dr_old = delta_radius,
|
|
441
|
|
- zh_old = delta_height,
|
|
442
|
|
- ta_old[ABC] = {
|
|
|
500
|
+ r_old = delta_radius,
|
|
|
501
|
+ h_old = delta_height,
|
|
|
502
|
+ a_old[ABC] = {
|
|
443
|
503
|
delta_tower_angle_trim[A_AXIS],
|
|
444
|
504
|
delta_tower_angle_trim[B_AXIS],
|
|
445
|
505
|
delta_tower_angle_trim[C_AXIS]
|
|
|
@@ -447,10 +507,10 @@ void GcodeSuite::G33() {
|
|
447
|
507
|
|
|
448
|
508
|
SERIAL_PROTOCOLLNPGM("G33 Auto Calibrate");
|
|
449
|
509
|
|
|
450
|
|
- if (!_1p_calibration && !_0p_calibration) { // test if the outer radius is reachable
|
|
|
510
|
+ if (!_1p_calibration && !_0p_calibration) { // test if the outer radius is reachable
|
|
451
|
511
|
LOOP_CAL_RAD(axis) {
|
|
452
|
512
|
const float a = RADIANS(210 + (360 / NPP) * (axis - 1)),
|
|
453
|
|
- r = delta_calibration_radius * (1 + (_7p_9_centre ? 0.1 : 0.0));
|
|
|
513
|
+ r = delta_calibration_radius;
|
|
454
|
514
|
if (!position_is_reachable(cos(a) * r, sin(a) * r)) {
|
|
455
|
515
|
SERIAL_PROTOCOLLNPGM("?(M665 B)ed radius is implausible.");
|
|
456
|
516
|
return;
|
|
|
@@ -458,159 +518,137 @@ void GcodeSuite::G33() {
|
|
458
|
518
|
}
|
|
459
|
519
|
}
|
|
460
|
520
|
|
|
461
|
|
- stepper.synchronize();
|
|
462
|
|
- #if HAS_LEVELING
|
|
463
|
|
- reset_bed_level(); // After calibration bed-level data is no longer valid
|
|
464
|
|
- #endif
|
|
465
|
|
-
|
|
466
|
|
- #if HOTENDS > 1
|
|
467
|
|
- const uint8_t old_tool_index = active_extruder;
|
|
468
|
|
- tool_change(0, 0, true);
|
|
469
|
|
- #define G33_CLEANUP() G33_cleanup(old_tool_index)
|
|
470
|
|
- #else
|
|
471
|
|
- #define G33_CLEANUP() G33_cleanup()
|
|
472
|
|
- #endif
|
|
473
|
|
-
|
|
474
|
|
- setup_for_endstop_or_probe_move();
|
|
475
|
|
- endstops.enable(true);
|
|
476
|
|
- if (!_0p_calibration) {
|
|
477
|
|
- if (!home_delta())
|
|
478
|
|
- return;
|
|
479
|
|
- endstops.not_homing();
|
|
480
|
|
- }
|
|
481
|
|
-
|
|
482
|
|
- if (auto_tune) {
|
|
483
|
|
- #if HAS_BED_PROBE
|
|
484
|
|
- G33_auto_tune();
|
|
485
|
|
- #else
|
|
486
|
|
- SERIAL_PROTOCOLLNPGM("A probe is needed for auto-tune");
|
|
487
|
|
- #endif
|
|
488
|
|
- G33_CLEANUP();
|
|
489
|
|
- return;
|
|
490
|
|
- }
|
|
491
|
|
-
|
|
492
|
521
|
// Report settings
|
|
493
|
522
|
|
|
494
|
|
- PGM_P checkingac = PSTR("Checking... AC"); // TODO: Make translatable string
|
|
|
523
|
+ const char *checkingac = PSTR("Checking... AC");
|
|
495
|
524
|
serialprintPGM(checkingac);
|
|
496
|
525
|
if (verbose_level == 0) SERIAL_PROTOCOLPGM(" (DRY-RUN)");
|
|
|
526
|
+ if (set_up) SERIAL_PROTOCOLPGM(" (SET-UP)");
|
|
497
|
527
|
SERIAL_EOL();
|
|
498
|
|
- lcd_setstatusPGM(checkingac);
|
|
|
528
|
+ char mess[11];
|
|
|
529
|
+ strcpy_P(mess, checkingac);
|
|
|
530
|
+ lcd_setstatus(mess);
|
|
499
|
531
|
|
|
500
|
|
- print_G33_settings(_endstop_results, _angle_results);
|
|
|
532
|
+ print_calibration_settings(_endstop_results, _angle_results);
|
|
501
|
533
|
|
|
502
|
|
- do {
|
|
|
534
|
+ ac_setup(!_0p_calibration && !_1p_calibration);
|
|
503
|
535
|
|
|
504
|
|
- float z_at_pt[NPP + 1] = { 0.0 };
|
|
|
536
|
+ if (!_0p_calibration)
|
|
|
537
|
+ if (!ac_home()) return;
|
|
|
538
|
+
|
|
|
539
|
+ do { // start iterations
|
|
505
|
540
|
|
|
506
|
|
- test_precision = zero_std_dev;
|
|
|
541
|
+ float z_at_pt[NPP + 1] = { 0.0 };
|
|
507
|
542
|
|
|
|
543
|
+ test_precision = zero_std_dev_old != 999.0 ? (zero_std_dev + zero_std_dev_old) / 2 : zero_std_dev;
|
|
508
|
544
|
iterations++;
|
|
509
|
545
|
|
|
510
|
546
|
// Probe the points
|
|
511
|
|
-
|
|
512
|
|
- zero_std_dev = probe_G33_points(z_at_pt, probe_points, towers_set, stow_after_each);
|
|
513
|
|
- if (isnan(zero_std_dev)) {
|
|
514
|
|
- SERIAL_PROTOCOLPGM("Correct delta_radius with M665 R or end-stops with M666 X Y Z");
|
|
515
|
|
- SERIAL_EOL();
|
|
516
|
|
- return G33_CLEANUP();
|
|
|
547
|
+ zero_std_dev_old = zero_std_dev;
|
|
|
548
|
+ if (!probe_calibration_points(z_at_pt, probe_points, towers_set, stow_after_each, set_up)) {
|
|
|
549
|
+ SERIAL_PROTOCOLLNPGM("Correct delta settings with M665 and M666");
|
|
|
550
|
+ return AC_CLEANUP();
|
|
517
|
551
|
}
|
|
|
552
|
+ zero_std_dev = std_dev_points(z_at_pt, _0p_calibration, _1p_calibration, _4p_calibration, _4p_opposite_points);
|
|
518
|
553
|
|
|
519
|
554
|
// Solve matrices
|
|
520
|
555
|
|
|
521
|
556
|
if ((zero_std_dev < test_precision || iterations <= force_iterations) && zero_std_dev > calibration_precision) {
|
|
|
557
|
+
|
|
|
558
|
+ #if !HAS_BED_PROBE
|
|
|
559
|
+ test_precision = 0.00; // forced end
|
|
|
560
|
+ #endif
|
|
|
561
|
+
|
|
522
|
562
|
if (zero_std_dev < zero_std_dev_min) {
|
|
|
563
|
+ // set roll-back point
|
|
523
|
564
|
COPY(e_old, delta_endstop_adj);
|
|
524
|
|
- dr_old = delta_radius;
|
|
525
|
|
- zh_old = delta_height;
|
|
526
|
|
- COPY(ta_old, delta_tower_angle_trim);
|
|
|
565
|
+ r_old = delta_radius;
|
|
|
566
|
+ h_old = delta_height;
|
|
|
567
|
+ COPY(a_old, delta_tower_angle_trim);
|
|
527
|
568
|
}
|
|
528
|
569
|
|
|
529
|
|
- float e_delta[ABC] = { 0.0 }, r_delta = 0.0, t_delta[ABC] = { 0.0 };
|
|
530
|
|
- const float r_diff = delta_radius - delta_calibration_radius,
|
|
531
|
|
- h_factor = 1 / 6.0 *
|
|
532
|
|
- #ifdef H_FACTOR
|
|
533
|
|
- (H_FACTOR), // Set in Configuration.h
|
|
534
|
|
- #else
|
|
535
|
|
- (1.00 + r_diff * 0.001), // 1.02 for r_diff = 20mm
|
|
536
|
|
- #endif
|
|
537
|
|
- r_factor = 1 / 6.0 *
|
|
538
|
|
- #ifdef R_FACTOR
|
|
539
|
|
- -(R_FACTOR), // Set in Configuration.h
|
|
540
|
|
- #else
|
|
541
|
|
- -(1.75 + 0.005 * r_diff + 0.001 * sq(r_diff)), // 2.25 for r_diff = 20mm
|
|
542
|
|
- #endif
|
|
543
|
|
- a_factor = 1 / 6.0 *
|
|
544
|
|
- #ifdef A_FACTOR
|
|
545
|
|
- (A_FACTOR); // Set in Configuration.h
|
|
546
|
|
- #else
|
|
547
|
|
- (66.66 / delta_calibration_radius); // 0.83 for cal_rd = 80mm
|
|
548
|
|
- #endif
|
|
549
|
|
-
|
|
550
|
|
- #define ZP(N,I) ((N) * z_at_pt[I])
|
|
551
|
|
- #define Z6(I) ZP(6, I)
|
|
|
570
|
+ float e_delta[ABC] = { 0.0 },
|
|
|
571
|
+ r_delta = 0.0,
|
|
|
572
|
+ t_delta[ABC] = { 0.0 };
|
|
|
573
|
+
|
|
|
574
|
+ /**
|
|
|
575
|
+ * convergence matrices:
|
|
|
576
|
+ * see https://github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for
|
|
|
577
|
+ * - definition of the matrix scaling parameters
|
|
|
578
|
+ * - matrices for 4 and 7 point calibration
|
|
|
579
|
+ */
|
|
|
580
|
+ #define ZP(N,I) ((N) * z_at_pt[I] / 4.0) // 4.0 = divider to normalize to integers
|
|
|
581
|
+ #define Z12(I) ZP(12, I)
|
|
552
|
582
|
#define Z4(I) ZP(4, I)
|
|
553
|
583
|
#define Z2(I) ZP(2, I)
|
|
554
|
584
|
#define Z1(I) ZP(1, I)
|
|
|
585
|
+ #define Z0(I) ZP(0, I)
|
|
555
|
586
|
|
|
556
|
|
- #if !HAS_BED_PROBE
|
|
557
|
|
- test_precision = 0.00; // forced end
|
|
558
|
|
- #endif
|
|
|
587
|
+ // calculate factors
|
|
|
588
|
+ const float cr_old = delta_calibration_radius;
|
|
|
589
|
+ if (_7p_9_center) delta_calibration_radius *= 0.9;
|
|
|
590
|
+ h_factor = auto_tune_h();
|
|
|
591
|
+ r_factor = auto_tune_r();
|
|
|
592
|
+ a_factor = auto_tune_a();
|
|
|
593
|
+ delta_calibration_radius = cr_old;
|
|
559
|
594
|
|
|
560
|
595
|
switch (probe_points) {
|
|
|
596
|
+ case -1:
|
|
|
597
|
+ #if HAS_BED_PROBE
|
|
|
598
|
+ zprobe_zoffset += probe_z_shift(z_at_pt[CEN]);
|
|
|
599
|
+ #endif
|
|
|
600
|
+
|
|
561
|
601
|
case 0:
|
|
562
|
602
|
test_precision = 0.00; // forced end
|
|
563
|
603
|
break;
|
|
564
|
604
|
|
|
565
|
605
|
case 1:
|
|
566
|
606
|
test_precision = 0.00; // forced end
|
|
567
|
|
- LOOP_XYZ(axis) e_delta[axis] = Z1(CEN);
|
|
|
607
|
+ LOOP_XYZ(axis) e_delta[axis] = +Z4(CEN);
|
|
568
|
608
|
break;
|
|
569
|
609
|
|
|
570
|
610
|
case 2:
|
|
571
|
|
- if (towers_set) {
|
|
572
|
|
- e_delta[A_AXIS] = (Z6(CEN) +Z4(__A) -Z2(__B) -Z2(__C)) * h_factor;
|
|
573
|
|
- e_delta[B_AXIS] = (Z6(CEN) -Z2(__A) +Z4(__B) -Z2(__C)) * h_factor;
|
|
574
|
|
- e_delta[C_AXIS] = (Z6(CEN) -Z2(__A) -Z2(__B) +Z4(__C)) * h_factor;
|
|
575
|
|
- r_delta = (Z6(CEN) -Z2(__A) -Z2(__B) -Z2(__C)) * r_factor;
|
|
|
611
|
+ if (towers_set) { // see 4 point calibration (towers) matrix
|
|
|
612
|
+ e_delta[A_AXIS] = (+Z4(__A) -Z2(__B) -Z2(__C)) * h_factor +Z4(CEN);
|
|
|
613
|
+ e_delta[B_AXIS] = (-Z2(__A) +Z4(__B) -Z2(__C)) * h_factor +Z4(CEN);
|
|
|
614
|
+ e_delta[C_AXIS] = (-Z2(__A) -Z2(__B) +Z4(__C)) * h_factor +Z4(CEN);
|
|
|
615
|
+ r_delta = (+Z4(__A) +Z4(__B) +Z4(__C) -Z12(CEN)) * r_factor;
|
|
576
|
616
|
}
|
|
577
|
|
- else {
|
|
578
|
|
- e_delta[A_AXIS] = (Z6(CEN) -Z4(_BC) +Z2(_CA) +Z2(_AB)) * h_factor;
|
|
579
|
|
- e_delta[B_AXIS] = (Z6(CEN) +Z2(_BC) -Z4(_CA) +Z2(_AB)) * h_factor;
|
|
580
|
|
- e_delta[C_AXIS] = (Z6(CEN) +Z2(_BC) +Z2(_CA) -Z4(_AB)) * h_factor;
|
|
581
|
|
- r_delta = (Z6(CEN) -Z2(_BC) -Z2(_CA) -Z2(_AB)) * r_factor;
|
|
|
617
|
+ else { // see 4 point calibration (opposites) matrix
|
|
|
618
|
+ e_delta[A_AXIS] = (-Z4(_BC) +Z2(_CA) +Z2(_AB)) * h_factor +Z4(CEN);
|
|
|
619
|
+ e_delta[B_AXIS] = (+Z2(_BC) -Z4(_CA) +Z2(_AB)) * h_factor +Z4(CEN);
|
|
|
620
|
+ e_delta[C_AXIS] = (+Z2(_BC) +Z2(_CA) -Z4(_AB)) * h_factor +Z4(CEN);
|
|
|
621
|
+ r_delta = (+Z4(_BC) +Z4(_CA) +Z4(_AB) -Z12(CEN)) * r_factor;
|
|
582
|
622
|
}
|
|
583
|
623
|
break;
|
|
584
|
624
|
|
|
585
|
|
- default:
|
|
586
|
|
- e_delta[A_AXIS] = (Z6(CEN) +Z2(__A) -Z1(__B) -Z1(__C) -Z2(_BC) +Z1(_CA) +Z1(_AB)) * h_factor;
|
|
587
|
|
- e_delta[B_AXIS] = (Z6(CEN) -Z1(__A) +Z2(__B) -Z1(__C) +Z1(_BC) -Z2(_CA) +Z1(_AB)) * h_factor;
|
|
588
|
|
- e_delta[C_AXIS] = (Z6(CEN) -Z1(__A) -Z1(__B) +Z2(__C) +Z1(_BC) +Z1(_CA) -Z2(_AB)) * h_factor;
|
|
589
|
|
- r_delta = (Z6(CEN) -Z1(__A) -Z1(__B) -Z1(__C) -Z1(_BC) -Z1(_CA) -Z1(_AB)) * r_factor;
|
|
590
|
|
-
|
|
591
|
|
- if (towers_set) {
|
|
592
|
|
- t_delta[A_AXIS] = ( -Z4(__B) +Z4(__C) -Z4(_CA) +Z4(_AB)) * a_factor;
|
|
593
|
|
- t_delta[B_AXIS] = ( Z4(__A) -Z4(__C) +Z4(_BC) -Z4(_AB)) * a_factor;
|
|
594
|
|
- t_delta[C_AXIS] = (-Z4(__A) +Z4(__B) -Z4(_BC) +Z4(_CA) ) * a_factor;
|
|
595
|
|
- e_delta[A_AXIS] += (t_delta[B_AXIS] - t_delta[C_AXIS]) / 4.5;
|
|
596
|
|
- e_delta[B_AXIS] += (t_delta[C_AXIS] - t_delta[A_AXIS]) / 4.5;
|
|
597
|
|
- e_delta[C_AXIS] += (t_delta[A_AXIS] - t_delta[B_AXIS]) / 4.5;
|
|
|
625
|
+ default: // see 7 point calibration (towers & opposites) matrix
|
|
|
626
|
+ e_delta[A_AXIS] = (+Z2(__A) -Z1(__B) -Z1(__C) -Z2(_BC) +Z1(_CA) +Z1(_AB)) * h_factor +Z4(CEN);
|
|
|
627
|
+ e_delta[B_AXIS] = (-Z1(__A) +Z2(__B) -Z1(__C) +Z1(_BC) -Z2(_CA) +Z1(_AB)) * h_factor +Z4(CEN);
|
|
|
628
|
+ e_delta[C_AXIS] = (-Z1(__A) -Z1(__B) +Z2(__C) +Z1(_BC) +Z1(_CA) -Z2(_AB)) * h_factor +Z4(CEN);
|
|
|
629
|
+ r_delta = (+Z2(__A) +Z2(__B) +Z2(__C) +Z2(_BC) +Z2(_CA) +Z2(_AB) -Z12(CEN)) * r_factor;
|
|
|
630
|
+
|
|
|
631
|
+ if (towers_set) { // see 7 point tower angle calibration (towers & opposites) matrix
|
|
|
632
|
+ t_delta[A_AXIS] = (+Z0(__A) -Z4(__B) +Z4(__C) +Z0(_BC) -Z4(_CA) +Z4(_AB) +Z0(CEN)) * a_factor;
|
|
|
633
|
+ t_delta[B_AXIS] = (+Z4(__A) +Z0(__B) -Z4(__C) +Z4(_BC) +Z0(_CA) -Z4(_AB) +Z0(CEN)) * a_factor;
|
|
|
634
|
+ t_delta[C_AXIS] = (-Z4(__A) +Z4(__B) +Z0(__C) -Z4(_BC) +Z4(_CA) +Z0(_AB) +Z0(CEN)) * a_factor;
|
|
598
|
635
|
}
|
|
599
|
636
|
break;
|
|
600
|
637
|
}
|
|
601
|
|
-
|
|
602
|
638
|
LOOP_XYZ(axis) delta_endstop_adj[axis] += e_delta[axis];
|
|
603
|
639
|
delta_radius += r_delta;
|
|
604
|
640
|
LOOP_XYZ(axis) delta_tower_angle_trim[axis] += t_delta[axis];
|
|
605
|
641
|
}
|
|
606
|
|
- else if (zero_std_dev >= test_precision) { // step one back
|
|
|
642
|
+ else if (zero_std_dev >= test_precision) {
|
|
|
643
|
+ // roll back
|
|
607
|
644
|
COPY(delta_endstop_adj, e_old);
|
|
608
|
|
- delta_radius = dr_old;
|
|
609
|
|
- delta_height = zh_old;
|
|
610
|
|
- COPY(delta_tower_angle_trim, ta_old);
|
|
|
645
|
+ delta_radius = r_old;
|
|
|
646
|
+ delta_height = h_old;
|
|
|
647
|
+ COPY(delta_tower_angle_trim, a_old);
|
|
611
|
648
|
}
|
|
612
|
649
|
|
|
613
|
650
|
if (verbose_level != 0) { // !dry run
|
|
|
651
|
+
|
|
614
|
652
|
// normalise angles to least squares
|
|
615
|
653
|
if (_angle_results) {
|
|
616
|
654
|
float a_sum = 0.0;
|
|
|
@@ -628,15 +666,15 @@ void GcodeSuite::G33() {
|
|
628
|
666
|
|
|
629
|
667
|
// print report
|
|
630
|
668
|
|
|
631
|
|
- if (verbose_level > 2)
|
|
632
|
|
- print_G33_results(z_at_pt, _tower_results, _opposite_results);
|
|
|
669
|
+ if (verbose_level == 3)
|
|
|
670
|
+ print_calibration_results(z_at_pt, _tower_results, _opposite_results);
|
|
633
|
671
|
|
|
634
|
|
- if (verbose_level != 0) { // !dry run
|
|
635
|
|
- if ((zero_std_dev >= test_precision && iterations > force_iterations) || zero_std_dev <= calibration_precision) { // end iterations
|
|
|
672
|
+ if (verbose_level != 0) { // !dry run
|
|
|
673
|
+ if ((zero_std_dev >= test_precision && iterations > force_iterations) || zero_std_dev <= calibration_precision) { // end iterations
|
|
636
|
674
|
SERIAL_PROTOCOLPGM("Calibration OK");
|
|
637
|
675
|
SERIAL_PROTOCOL_SP(32);
|
|
638
|
676
|
#if HAS_BED_PROBE
|
|
639
|
|
- if (zero_std_dev >= test_precision && !_1p_calibration)
|
|
|
677
|
+ if (zero_std_dev >= test_precision && !_1p_calibration && !_0p_calibration)
|
|
640
|
678
|
SERIAL_PROTOCOLPGM("rolling back.");
|
|
641
|
679
|
else
|
|
642
|
680
|
#endif
|
|
|
@@ -652,11 +690,11 @@ void GcodeSuite::G33() {
|
|
652
|
690
|
else
|
|
653
|
691
|
sprintf_P(&mess[15], PSTR("%03i.x"), (int)round(zero_std_dev_min));
|
|
654
|
692
|
lcd_setstatus(mess);
|
|
655
|
|
- print_G33_settings(_endstop_results, _angle_results);
|
|
|
693
|
+ print_calibration_settings(_endstop_results, _angle_results);
|
|
656
|
694
|
serialprintPGM(save_message);
|
|
657
|
695
|
SERIAL_EOL();
|
|
658
|
696
|
}
|
|
659
|
|
- else { // !end iterations
|
|
|
697
|
+ else { // !end iterations
|
|
660
|
698
|
char mess[15];
|
|
661
|
699
|
if (iterations < 31)
|
|
662
|
700
|
sprintf_P(mess, PSTR("Iteration : %02i"), (int)iterations);
|
|
|
@@ -669,11 +707,11 @@ void GcodeSuite::G33() {
|
|
669
|
707
|
SERIAL_EOL();
|
|
670
|
708
|
lcd_setstatus(mess);
|
|
671
|
709
|
if (verbose_level > 1)
|
|
672
|
|
- print_G33_settings(_endstop_results, _angle_results);
|
|
|
710
|
+ print_calibration_settings(_endstop_results, _angle_results);
|
|
673
|
711
|
}
|
|
674
|
712
|
}
|
|
675
|
|
- else { // dry run
|
|
676
|
|
- PGM_P enddryrun = PSTR("End DRY-RUN");
|
|
|
713
|
+ else { // dry run
|
|
|
714
|
+ const char *enddryrun = PSTR("End DRY-RUN");
|
|
677
|
715
|
serialprintPGM(enddryrun);
|
|
678
|
716
|
SERIAL_PROTOCOL_SP(35);
|
|
679
|
717
|
SERIAL_PROTOCOLPGM("std dev:");
|
|
|
@@ -689,16 +727,11 @@ void GcodeSuite::G33() {
|
|
689
|
727
|
sprintf_P(&mess[15], PSTR("%03i.x"), (int)round(zero_std_dev));
|
|
690
|
728
|
lcd_setstatus(mess);
|
|
691
|
729
|
}
|
|
692
|
|
-
|
|
693
|
|
- endstops.enable(true);
|
|
694
|
|
- if (!home_delta())
|
|
695
|
|
- return;
|
|
696
|
|
- endstops.not_homing();
|
|
697
|
|
-
|
|
|
730
|
+ if (!ac_home()) return;
|
|
698
|
731
|
}
|
|
699
|
732
|
while (((zero_std_dev < test_precision && iterations < 31) || iterations <= force_iterations) && zero_std_dev > calibration_precision);
|
|
700
|
733
|
|
|
701
|
|
- G33_CLEANUP();
|
|
|
734
|
+ AC_CLEANUP();
|
|
702
|
735
|
}
|
|
703
|
736
|
|
|
704
|
737
|
#endif // DELTA_AUTO_CALIBRATION
|
Luc Van Daele
7 years ago