Rui Caridade
5 years ago
committed by
GitHub
GitHub
5 changed files with 229 additions and 2 deletions
@ -0,0 +1,192 @@ |
|||||
|
/**
|
||||
|
* Marlin 3D Printer Firmware |
||||
|
* Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
|
||||
|
* |
||||
|
* Based on Sprinter and grbl. |
||||
|
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm |
||||
|
* |
||||
|
* This program is free software: you can redistribute it and/or modify |
||||
|
* it under the terms of the GNU General Public License as published by |
||||
|
* the Free Software Foundation, either version 3 of the License, or |
||||
|
* (at your option) any later version. |
||||
|
* |
||||
|
* This program is distributed in the hope that it will be useful, |
||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
|
* GNU General Public License for more details. |
||||
|
* |
||||
|
* You should have received a copy of the GNU General Public License |
||||
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
* |
||||
|
*/ |
||||
|
|
||||
|
#include "../../inc/MarlinConfig.h" |
||||
|
|
||||
|
#if ENABLED(ASSISTED_TRAMMING) |
||||
|
|
||||
|
#include "../gcode.h" |
||||
|
#include "../../module/planner.h" |
||||
|
#include "../../module/probe.h" |
||||
|
#include "../../feature/bedlevel/bedlevel.h" |
||||
|
|
||||
|
#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) |
||||
|
#include "../../core/debug_out.h" |
||||
|
|
||||
|
constexpr xy_pos_t screws_tilt_adjust_pos[] = TRAMMING_POINT_XY; |
||||
|
|
||||
|
static PGMSTR(point_name_1, TRAMMING_POINT_NAME_1); |
||||
|
static PGMSTR(point_name_2, TRAMMING_POINT_NAME_2); |
||||
|
static PGMSTR(point_name_3, TRAMMING_POINT_NAME_3); |
||||
|
#ifdef TRAMMING_POINT_NAME_4 |
||||
|
static PGMSTR(point_name_4, TRAMMING_POINT_NAME_4); |
||||
|
#ifdef TRAMMING_POINT_NAME_5 |
||||
|
static PGMSTR(point_name_5, TRAMMING_POINT_NAME_5); |
||||
|
#endif |
||||
|
#endif |
||||
|
|
||||
|
static PGM_P const tramming_point_name[] PROGMEM = { |
||||
|
point_name_1, point_name_2, point_name_3 |
||||
|
#ifdef TRAMMING_POINT_NAME_4 |
||||
|
, point_name_4 |
||||
|
#ifdef TRAMMING_POINT_NAME_5 |
||||
|
, point_name_5 |
||||
|
#endif |
||||
|
#endif |
||||
|
}; |
||||
|
|
||||
|
#define G35_PROBE_COUNT COUNT(screws_tilt_adjust_pos) |
||||
|
|
||||
|
#if !WITHIN(TRAMMING_SCREW_THREAD, 30, 51) || TRAMMING_SCREW_THREAD % 10 > 1 |
||||
|
#error "TRAMMING_SCREW_THREAD must be equal to 30, 31, 40, 41, 50, or 51." |
||||
|
#endif |
||||
|
|
||||
|
static_assert(G35_PROBE_COUNT > 2, "TRAMMING_POINT_XY requires at least 3 XY positions."); |
||||
|
|
||||
|
/**
|
||||
|
* G35: Read bed corners to help adjust bed screws |
||||
|
* |
||||
|
* S<screw_thread> |
||||
|
* |
||||
|
* Screw thread: 30 - Clockwise M3 |
||||
|
* 31 - Counter-Clockwise M3 |
||||
|
* 40 - Clockwise M4 |
||||
|
* 41 - Counter-Clockwise M4 |
||||
|
* 50 - Clockwise M5 |
||||
|
* 51 - Counter-Clockwise M5 |
||||
|
**/ |
||||
|
void GcodeSuite::G35() { |
||||
|
if (DEBUGGING(LEVELING)) { |
||||
|
DEBUG_ECHOLNPGM(">>> G35"); |
||||
|
log_machine_info(); |
||||
|
} |
||||
|
|
||||
|
float z_measured[G35_PROBE_COUNT] = { 0 }; |
||||
|
|
||||
|
const uint8_t screw_thread = parser.byteval('S', TRAMMING_SCREW_THREAD); |
||||
|
if (!WITHIN(screw_thread, 30, 51) || screw_thread % 10 > 1) { |
||||
|
SERIAL_ECHOLNPGM("?(S)crew thread must be 30, 31, 40, 41, 50, or 51."); |
||||
|
goto EXIT_G35; |
||||
|
} |
||||
|
|
||||
|
// Wait for planner moves to finish!
|
||||
|
planner.synchronize(); |
||||
|
|
||||
|
// Disable the leveling matrix before auto-aligning
|
||||
|
#if HAS_LEVELING |
||||
|
TERN_(RESTORE_LEVELING_AFTER_G35, const bool leveling_was_active = planner.leveling_active); |
||||
|
set_bed_leveling_enabled(false); |
||||
|
#endif |
||||
|
|
||||
|
#if ENABLED(CNC_WORKSPACE_PLANES) |
||||
|
workspace_plane = PLANE_XY; |
||||
|
#endif |
||||
|
|
||||
|
// Always home with tool 0 active
|
||||
|
#if HAS_MULTI_HOTEND |
||||
|
const uint8_t old_tool_index = active_extruder; |
||||
|
tool_change(0, true); |
||||
|
#endif |
||||
|
|
||||
|
#if HAS_DUPLICATION_MODE |
||||
|
extruder_duplication_enabled = false; |
||||
|
#endif |
||||
|
|
||||
|
// Home all before this procedure
|
||||
|
home_all_axes(); |
||||
|
|
||||
|
bool err_break = false; |
||||
|
|
||||
|
// Probe all positions
|
||||
|
LOOP_L_N(i, G35_PROBE_COUNT) { |
||||
|
|
||||
|
// In BLTOUCH HS mode, the probe travels in a deployed state.
|
||||
|
// Users of G35 might have a badly misaligned bed, so raise Z by the
|
||||
|
// length of the deployed pin (BLTOUCH stroke < 7mm)
|
||||
|
current_position.z = (Z_CLEARANCE_BETWEEN_PROBES) + (7 * ENABLED(BLTOUCH_HS_MODE)); |
||||
|
|
||||
|
const float z_probed_height = probe.probe_at_point(screws_tilt_adjust_pos[i], PROBE_PT_RAISE, 0, true); |
||||
|
|
||||
|
if (isnan(z_probed_height)) { |
||||
|
SERIAL_ECHOLNPAIR("G35 failed at point ", int(i), " (", tramming_point_name[i], ")" |
||||
|
" X", screws_tilt_adjust_pos[i].x, |
||||
|
" Y", screws_tilt_adjust_pos[i].y); |
||||
|
err_break = true; |
||||
|
break; |
||||
|
} |
||||
|
|
||||
|
if (DEBUGGING(LEVELING)) |
||||
|
DEBUG_ECHOLNPAIR("Probing point ", int(i), " (", tramming_point_name[i], ")" |
||||
|
" X", screws_tilt_adjust_pos[i].x, |
||||
|
" Y", screws_tilt_adjust_pos[i].y, |
||||
|
" Z", z_probed_height); |
||||
|
|
||||
|
z_measured[i] = z_probed_height; |
||||
|
} |
||||
|
|
||||
|
if (!err_break) { |
||||
|
const float threads_factor[] = { 0.5, 0.7, 0.8 }; |
||||
|
|
||||
|
// Calculate adjusts
|
||||
|
LOOP_S_L_N(i, 1, G35_PROBE_COUNT) { |
||||
|
const float diff = z_measured[0] - z_measured[i], |
||||
|
adjust = abs(diff) < 0.001f ? 0 : diff / threads_factor[(screw_thread - 30) / 10]; |
||||
|
|
||||
|
const int full_turns = trunc(adjust); |
||||
|
const float decimal_part = adjust - float(full_turns); |
||||
|
const int minutes = trunc(decimal_part * 60.0f); |
||||
|
|
||||
|
SERIAL_ECHOPAIR("Turn ", tramming_point_name[i], |
||||
|
" ", (screw_thread & 1) == (adjust > 0) ? "Counter-Clockwise" : "Clockwise", |
||||
|
"by ", abs(full_turns), " turns"); |
||||
|
if (minutes) SERIAL_ECHOPAIR(" and ", abs(minutes), " minutes"); |
||||
|
SERIAL_EOL(); |
||||
|
} |
||||
|
} |
||||
|
else |
||||
|
SERIAL_ECHOLNPGM("G35 aborted."); |
||||
|
|
||||
|
// Restore the active tool after homing
|
||||
|
#if HAS_MULTI_HOTEND |
||||
|
tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER)); // Fetch previous toolhead if not PARKING_EXTRUDER
|
||||
|
#endif |
||||
|
|
||||
|
#if BOTH(HAS_LEVELING, RESTORE_LEVELING_AFTER_G35) |
||||
|
set_bed_leveling_enabled(leveling_was_active); |
||||
|
#endif |
||||
|
|
||||
|
// Stow the probe, as the last call to probe.probe_at_point(...) left
|
||||
|
// the probe deployed if it was successful.
|
||||
|
probe.stow(); |
||||
|
|
||||
|
// After this operation the Z position needs correction
|
||||
|
set_axis_not_trusted(Z_AXIS); |
||||
|
|
||||
|
// Home Z after the alignment procedure
|
||||
|
process_subcommands_now_P(PSTR("G28Z")); |
||||
|
|
||||
|
EXIT_G35: |
||||
|
|
||||
|
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< G35"); |
||||
|
} |
||||
|
|
||||
|
#endif // ASSISTED_TRAMMING
|
||||
Loading…
Reference in new issue