|
@@ -0,0 +1,192 @@
|
|
1
|
+/**
|
|
2
|
+ * Marlin 3D Printer Firmware
|
|
3
|
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
|
|
4
|
+ *
|
|
5
|
+ * Based on Sprinter and grbl.
|
|
6
|
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
|
|
7
|
+ *
|
|
8
|
+ * This program is free software: you can redistribute it and/or modify
|
|
9
|
+ * it under the terms of the GNU General Public License as published by
|
|
10
|
+ * the Free Software Foundation, either version 3 of the License, or
|
|
11
|
+ * (at your option) any later version.
|
|
12
|
+ *
|
|
13
|
+ * This program is distributed in the hope that it will be useful,
|
|
14
|
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
15
|
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
16
|
+ * GNU General Public License for more details.
|
|
17
|
+ *
|
|
18
|
+ * You should have received a copy of the GNU General Public License
|
|
19
|
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
20
|
+ *
|
|
21
|
+ */
|
|
22
|
+
|
|
23
|
+#include "../../inc/MarlinConfig.h"
|
|
24
|
+
|
|
25
|
+#if ENABLED(ASSISTED_TRAMMING)
|
|
26
|
+
|
|
27
|
+#include "../gcode.h"
|
|
28
|
+#include "../../module/planner.h"
|
|
29
|
+#include "../../module/probe.h"
|
|
30
|
+#include "../../feature/bedlevel/bedlevel.h"
|
|
31
|
+
|
|
32
|
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
|
|
33
|
+#include "../../core/debug_out.h"
|
|
34
|
+
|
|
35
|
+constexpr xy_pos_t screws_tilt_adjust_pos[] = TRAMMING_POINT_XY;
|
|
36
|
+
|
|
37
|
+static PGMSTR(point_name_1, TRAMMING_POINT_NAME_1);
|
|
38
|
+static PGMSTR(point_name_2, TRAMMING_POINT_NAME_2);
|
|
39
|
+static PGMSTR(point_name_3, TRAMMING_POINT_NAME_3);
|
|
40
|
+#ifdef TRAMMING_POINT_NAME_4
|
|
41
|
+ static PGMSTR(point_name_4, TRAMMING_POINT_NAME_4);
|
|
42
|
+ #ifdef TRAMMING_POINT_NAME_5
|
|
43
|
+ static PGMSTR(point_name_5, TRAMMING_POINT_NAME_5);
|
|
44
|
+ #endif
|
|
45
|
+#endif
|
|
46
|
+
|
|
47
|
+static PGM_P const tramming_point_name[] PROGMEM = {
|
|
48
|
+ point_name_1, point_name_2, point_name_3
|
|
49
|
+ #ifdef TRAMMING_POINT_NAME_4
|
|
50
|
+ , point_name_4
|
|
51
|
+ #ifdef TRAMMING_POINT_NAME_5
|
|
52
|
+ , point_name_5
|
|
53
|
+ #endif
|
|
54
|
+ #endif
|
|
55
|
+};
|
|
56
|
+
|
|
57
|
+#define G35_PROBE_COUNT COUNT(screws_tilt_adjust_pos)
|
|
58
|
+
|
|
59
|
+#if !WITHIN(TRAMMING_SCREW_THREAD, 30, 51) || TRAMMING_SCREW_THREAD % 10 > 1
|
|
60
|
+ #error "TRAMMING_SCREW_THREAD must be equal to 30, 31, 40, 41, 50, or 51."
|
|
61
|
+#endif
|
|
62
|
+
|
|
63
|
+static_assert(G35_PROBE_COUNT > 2, "TRAMMING_POINT_XY requires at least 3 XY positions.");
|
|
64
|
+
|
|
65
|
+/**
|
|
66
|
+ * G35: Read bed corners to help adjust bed screws
|
|
67
|
+ *
|
|
68
|
+ * S<screw_thread>
|
|
69
|
+ *
|
|
70
|
+ * Screw thread: 30 - Clockwise M3
|
|
71
|
+ * 31 - Counter-Clockwise M3
|
|
72
|
+ * 40 - Clockwise M4
|
|
73
|
+ * 41 - Counter-Clockwise M4
|
|
74
|
+ * 50 - Clockwise M5
|
|
75
|
+ * 51 - Counter-Clockwise M5
|
|
76
|
+ **/
|
|
77
|
+void GcodeSuite::G35() {
|
|
78
|
+ if (DEBUGGING(LEVELING)) {
|
|
79
|
+ DEBUG_ECHOLNPGM(">>> G35");
|
|
80
|
+ log_machine_info();
|
|
81
|
+ }
|
|
82
|
+
|
|
83
|
+ float z_measured[G35_PROBE_COUNT] = { 0 };
|
|
84
|
+
|
|
85
|
+ const uint8_t screw_thread = parser.byteval('S', TRAMMING_SCREW_THREAD);
|
|
86
|
+ if (!WITHIN(screw_thread, 30, 51) || screw_thread % 10 > 1) {
|
|
87
|
+ SERIAL_ECHOLNPGM("?(S)crew thread must be 30, 31, 40, 41, 50, or 51.");
|
|
88
|
+ goto EXIT_G35;
|
|
89
|
+ }
|
|
90
|
+
|
|
91
|
+ // Wait for planner moves to finish!
|
|
92
|
+ planner.synchronize();
|
|
93
|
+
|
|
94
|
+ // Disable the leveling matrix before auto-aligning
|
|
95
|
+ #if HAS_LEVELING
|
|
96
|
+ TERN_(RESTORE_LEVELING_AFTER_G35, const bool leveling_was_active = planner.leveling_active);
|
|
97
|
+ set_bed_leveling_enabled(false);
|
|
98
|
+ #endif
|
|
99
|
+
|
|
100
|
+ #if ENABLED(CNC_WORKSPACE_PLANES)
|
|
101
|
+ workspace_plane = PLANE_XY;
|
|
102
|
+ #endif
|
|
103
|
+
|
|
104
|
+ // Always home with tool 0 active
|
|
105
|
+ #if HAS_MULTI_HOTEND
|
|
106
|
+ const uint8_t old_tool_index = active_extruder;
|
|
107
|
+ tool_change(0, true);
|
|
108
|
+ #endif
|
|
109
|
+
|
|
110
|
+ #if HAS_DUPLICATION_MODE
|
|
111
|
+ extruder_duplication_enabled = false;
|
|
112
|
+ #endif
|
|
113
|
+
|
|
114
|
+ // Home all before this procedure
|
|
115
|
+ home_all_axes();
|
|
116
|
+
|
|
117
|
+ bool err_break = false;
|
|
118
|
+
|
|
119
|
+ // Probe all positions
|
|
120
|
+ LOOP_L_N(i, G35_PROBE_COUNT) {
|
|
121
|
+
|
|
122
|
+ // In BLTOUCH HS mode, the probe travels in a deployed state.
|
|
123
|
+ // Users of G35 might have a badly misaligned bed, so raise Z by the
|
|
124
|
+ // length of the deployed pin (BLTOUCH stroke < 7mm)
|
|
125
|
+ current_position.z = (Z_CLEARANCE_BETWEEN_PROBES) + (7 * ENABLED(BLTOUCH_HS_MODE));
|
|
126
|
+
|
|
127
|
+ const float z_probed_height = probe.probe_at_point(screws_tilt_adjust_pos[i], PROBE_PT_RAISE, 0, true);
|
|
128
|
+
|
|
129
|
+ if (isnan(z_probed_height)) {
|
|
130
|
+ SERIAL_ECHOLNPAIR("G35 failed at point ", int(i), " (", tramming_point_name[i], ")"
|
|
131
|
+ " X", screws_tilt_adjust_pos[i].x,
|
|
132
|
+ " Y", screws_tilt_adjust_pos[i].y);
|
|
133
|
+ err_break = true;
|
|
134
|
+ break;
|
|
135
|
+ }
|
|
136
|
+
|
|
137
|
+ if (DEBUGGING(LEVELING))
|
|
138
|
+ DEBUG_ECHOLNPAIR("Probing point ", int(i), " (", tramming_point_name[i], ")"
|
|
139
|
+ " X", screws_tilt_adjust_pos[i].x,
|
|
140
|
+ " Y", screws_tilt_adjust_pos[i].y,
|
|
141
|
+ " Z", z_probed_height);
|
|
142
|
+
|
|
143
|
+ z_measured[i] = z_probed_height;
|
|
144
|
+ }
|
|
145
|
+
|
|
146
|
+ if (!err_break) {
|
|
147
|
+ const float threads_factor[] = { 0.5, 0.7, 0.8 };
|
|
148
|
+
|
|
149
|
+ // Calculate adjusts
|
|
150
|
+ LOOP_S_L_N(i, 1, G35_PROBE_COUNT) {
|
|
151
|
+ const float diff = z_measured[0] - z_measured[i],
|
|
152
|
+ adjust = abs(diff) < 0.001f ? 0 : diff / threads_factor[(screw_thread - 30) / 10];
|
|
153
|
+
|
|
154
|
+ const int full_turns = trunc(adjust);
|
|
155
|
+ const float decimal_part = adjust - float(full_turns);
|
|
156
|
+ const int minutes = trunc(decimal_part * 60.0f);
|
|
157
|
+
|
|
158
|
+ SERIAL_ECHOPAIR("Turn ", tramming_point_name[i],
|
|
159
|
+ " ", (screw_thread & 1) == (adjust > 0) ? "Counter-Clockwise" : "Clockwise",
|
|
160
|
+ "by ", abs(full_turns), " turns");
|
|
161
|
+ if (minutes) SERIAL_ECHOPAIR(" and ", abs(minutes), " minutes");
|
|
162
|
+ SERIAL_EOL();
|
|
163
|
+ }
|
|
164
|
+ }
|
|
165
|
+ else
|
|
166
|
+ SERIAL_ECHOLNPGM("G35 aborted.");
|
|
167
|
+
|
|
168
|
+ // Restore the active tool after homing
|
|
169
|
+ #if HAS_MULTI_HOTEND
|
|
170
|
+ tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER)); // Fetch previous toolhead if not PARKING_EXTRUDER
|
|
171
|
+ #endif
|
|
172
|
+
|
|
173
|
+ #if BOTH(HAS_LEVELING, RESTORE_LEVELING_AFTER_G35)
|
|
174
|
+ set_bed_leveling_enabled(leveling_was_active);
|
|
175
|
+ #endif
|
|
176
|
+
|
|
177
|
+ // Stow the probe, as the last call to probe.probe_at_point(...) left
|
|
178
|
+ // the probe deployed if it was successful.
|
|
179
|
+ probe.stow();
|
|
180
|
+
|
|
181
|
+ // After this operation the Z position needs correction
|
|
182
|
+ set_axis_not_trusted(Z_AXIS);
|
|
183
|
+
|
|
184
|
+ // Home Z after the alignment procedure
|
|
185
|
+ process_subcommands_now_P(PSTR("G28Z"));
|
|
186
|
+
|
|
187
|
+ EXIT_G35:
|
|
188
|
+
|
|
189
|
+ if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< G35");
|
|
190
|
+}
|
|
191
|
+
|
|
192
|
+#endif // ASSISTED_TRAMMING
|