From 78e111a4d04b3e64077da769c03178d6185f69f7 Mon Sep 17 00:00:00 2001 From: Evan Felix Date: Thu, 25 Jul 2024 22:13:52 -0700 Subject: [PATCH] =?UTF-8?q?=F0=9F=9A=B8=20Implement=20G60=20/=20G61=20to?= =?UTF-8?q?=20spec=20(#27281)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Scott Lahteine --- Marlin/src/core/language.h | 4 +- Marlin/src/gcode/feature/pause/G60.cpp | 87 ++++++++++++++++++-------- Marlin/src/gcode/feature/pause/G61.cpp | 68 +++++++++++--------- Marlin/src/gcode/gcode.h | 2 +- Marlin/src/module/motion.cpp | 2 +- Marlin/src/module/motion.h | 2 +- 6 files changed, 107 insertions(+), 58 deletions(-) diff --git a/Marlin/src/core/language.h b/Marlin/src/core/language.h index 9d2cdca897e7..2736a7833ed5 100644 --- a/Marlin/src/core/language.h +++ b/Marlin/src/core/language.h @@ -162,8 +162,8 @@ #define STR_SOFT_MIN " Min: " #define STR_SOFT_MAX " Max: " -#define STR_SAVED_POS "Position saved" -#define STR_RESTORING_POS "Restoring position" +#define STR_SAVED_POSITION "Saved position #" +#define STR_RESTORING_POSITION "Restoring position #" #define STR_INVALID_POS_SLOT "Invalid slot. Total: " #define STR_DONE "Done." diff --git a/Marlin/src/gcode/feature/pause/G60.cpp b/Marlin/src/gcode/feature/pause/G60.cpp index aa74a57596d1..fe085200a9ca 100644 --- a/Marlin/src/gcode/feature/pause/G60.cpp +++ b/Marlin/src/gcode/feature/pause/G60.cpp @@ -30,41 +30,78 @@ #define DEBUG_OUT ENABLED(SAVED_POSITIONS_DEBUG) #include "../../../core/debug_out.h" +bool report_stored_position(const uint8_t slot) { + if (!did_save_position[slot]) return false; + const xyze_pos_t &pos = stored_position[slot]; + SERIAL_ECHO(STR_SAVED_POSITION, slot, C(':')); + #if NUM_AXES + SERIAL_ECHOPGM_P( + LIST_N(DOUBLE(NUM_AXES), + SP_X_LBL, pos.x, SP_Y_LBL, pos.y, SP_Z_LBL, pos.z, + SP_I_LBL, pos.i, SP_J_LBL, pos.j, SP_K_LBL, pos.k, + SP_U_LBL, pos.u, SP_V_LBL, pos.v, SP_W_LBL, pos.w + ) + ); + #endif + #if HAS_EXTRUDERS + SERIAL_ECHOPGM_P(SP_E_LBL, pos.e); + #endif + SERIAL_EOL(); + return true; +} + /** - * G60: Save current position + * G60: Saved Positions * - * S - Memory slot # (0-based) to save into (default 0) + * S - Save to a memory slot. (default 0) + * Q - Restore from a memory slot. (default 0) + * D - Delete a memory slot. With no number, delete all. */ void GcodeSuite::G60() { - const uint8_t slot = parser.byteval('S'); + // With no parameters report any saved positions + if (!parser.seen_any()) { + uint8_t count = 0; + for (uint8_t s = 0; s < SAVED_POSITIONS; ++s) + if (report_stored_position(s)) ++count; + if (!count) SERIAL_ECHOLNPGM("No Saved Positions"); + return; + } - if (slot >= SAVED_POSITIONS) { - SERIAL_ERROR_MSG(STR_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS)); + // Only one of these parameters is permitted + const uint8_t seenD = parser.seen_test('D'), + seenQ = parser.seen_test('Q'), + seenS = parser.seen_test('S'); + if (seenD + seenQ + seenS > 1) return; + + // G60 D : Delete all saved positions + if (seenD && !parser.seenval('D')) { + did_save_position.reset(); return; } - stored_position[slot] = current_position; - SBI(saved_slots[slot >> 3], slot & 0x07); + // G60 Dn / Q / S : Get the slot value + const uint8_t slot = parser.byteval(seenD ? 'D' : seenQ ? 'Q' : 'S'); - #if ENABLED(SAVED_POSITIONS_DEBUG) - { - const xyze_pos_t &pos = stored_position[slot]; - DEBUG_ECHOPGM(STR_SAVED_POS " S", slot, " :"); - #if NUM_AXES - DEBUG_ECHOPGM_P( - LIST_N(DOUBLE(NUM_AXES), - SP_X_LBL, pos.x, SP_Y_LBL, pos.y, SP_Z_LBL, pos.z, - SP_I_LBL, pos.i, SP_J_LBL, pos.j, SP_K_LBL, pos.k, - SP_U_LBL, pos.u, SP_V_LBL, pos.v, SP_W_LBL, pos.w - ) - ); - #endif - #if HAS_EXTRUDERS - DEBUG_ECHOPGM_P(SP_E_LBL, pos.e); - #endif - DEBUG_EOL(); + // G60 Q : Redirect to G61(slot) + if (seenQ) return G61(slot); + + // Valid slot number? + if (SAVED_POSITIONS < 256 && slot >= SAVED_POSITIONS) { + SERIAL_ERROR_MSG(STR_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS)); + return; } - #endif + + // G60 Dn + if (seenD) { + SERIAL_ECHOLNPGM(STR_SAVED_POSITION, slot, ": DELETED"); + did_save_position.clear(slot); + return; + } + + // G60 S + stored_position[slot] = current_position; + did_save_position.set(slot); + report_stored_position(slot); } #endif // SAVED_POSITIONS diff --git a/Marlin/src/gcode/feature/pause/G61.cpp b/Marlin/src/gcode/feature/pause/G61.cpp index 068e0b0e40ee..08b4a99b7de5 100644 --- a/Marlin/src/gcode/feature/pause/G61.cpp +++ b/Marlin/src/gcode/feature/pause/G61.cpp @@ -54,52 +54,64 @@ * * If no axes are specified then all axes are restored. */ -void GcodeSuite::G61() { +void GcodeSuite::G61(int8_t slot/*=-1*/) { - const uint8_t slot = parser.byteval('S'); + if (slot < 0) slot = parser.byteval('S'); - #define SYNC_E(POINT) TERN_(HAS_EXTRUDERS, planner.set_e_position_mm((destination.e = current_position.e = (POINT)))) + #define SYNC_E(E) planner.set_e_position_mm(current_position.e = (E)) - #if SAVED_POSITIONS < 256 - if (slot >= SAVED_POSITIONS) { - SERIAL_ERROR_MSG(STR_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS)); - return; - } - #endif + if (SAVED_POSITIONS < 256 && slot >= SAVED_POSITIONS) { + SERIAL_ERROR_MSG(STR_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS)); + return; + } // No saved position? No axes being restored? - if (!TEST(saved_slots[slot >> 3], slot & 0x07)) return; + if (!did_save_position[slot]) return; // Apply any given feedrate over 0.0 REMEMBER(saved, feedrate_mm_s); const float fr = parser.linearval('F'); if (fr > 0.0) feedrate_mm_s = MMM_TO_MMS(fr); + // No XYZ...E parameters, move to stored position + + float epos = stored_position[slot].e; if (!parser.seen_axis()) { - DEBUG_ECHOLNPGM("Default position restore"); + DEBUG_ECHOLNPGM(STR_RESTORING_POSITION, slot, " (all axes)"); + // Move to the saved position, all axes except E do_blocking_move_to(stored_position[slot], feedrate_mm_s); - SYNC_E(stored_position[slot].e); + // Just set E to the saved position without moving it + TERN_(HAS_EXTRUDERS, SYNC_E(stored_position[slot].e)); + report_current_position(); + return; } - else { - if (parser.seen(STR_AXES_MAIN)) { - DEBUG_ECHOPGM(STR_RESTORING_POS " S", slot); - LOOP_NUM_AXES(i) { - destination[i] = parser.seen(AXIS_CHAR(i)) - ? stored_position[slot][i] + parser.value_axis_units((AxisEnum)i) - : current_position[i]; + + // With XYZ...E return specified axes + optional offset + + DEBUG_ECHOPGM(STR_RESTORING_POSITION " S", slot); + + if (parser.seen(STR_AXES_MAIN)) { + destination = current_position; + LOOP_NUM_AXES(i) { + if (parser.seen(AXIS_CHAR(i))) { + destination[i] = stored_position[slot][i] + parser.value_axis_units((AxisEnum)i); DEBUG_ECHO(C(' '), C(AXIS_CHAR(i)), p_float_t(destination[i], 2)); } - DEBUG_EOL(); - // Move to the saved position - prepare_line_to_destination(); } - #if HAS_EXTRUDERS - if (parser.seen_test('E')) { - DEBUG_ECHOLNPGM(STR_RESTORING_POS " S", slot, " E", current_position.e, "=>", stored_position[slot].e); - SYNC_E(stored_position[slot].e); - } - #endif + prepare_line_to_destination(); } + + #if HAS_EXTRUDERS + if (parser.seen('E')) { + epos += parser.value_axis_units(E_AXIS); + DEBUG_ECHOPGM(" E", epos); + SYNC_E(epos); + } + #endif + + DEBUG_EOL(); + + report_current_position(); } #endif // SAVED_POSITIONS diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h index bb2f0099949b..7ec3e0088817 100644 --- a/Marlin/src/gcode/gcode.h +++ b/Marlin/src/gcode/gcode.h @@ -606,7 +606,7 @@ class GcodeSuite { #if SAVED_POSITIONS static void G60(); - static void G61(); + static void G61(int8_t slot=-1); #endif #if ENABLED(GCODE_MOTION_MODES) diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index 4bbb182bc345..a4e69c5be7ca 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -104,7 +104,7 @@ xyze_pos_t destination; // {0} // G60/G61 Position Save and Return #if SAVED_POSITIONS - uint8_t saved_slots[(SAVED_POSITIONS + 7) >> 3]; + Flags did_save_position; xyze_pos_t stored_position[SAVED_POSITIONS]; #endif diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h index bb8f36446b0c..88fd69fbd06e 100644 --- a/Marlin/src/module/motion.h +++ b/Marlin/src/module/motion.h @@ -50,7 +50,7 @@ extern xyze_pos_t current_position, // High-level current tool position // G60/G61 Position Save and Return #if SAVED_POSITIONS - extern uint8_t saved_slots[(SAVED_POSITIONS + 7) >> 3]; // TODO: Add support for HAS_I_AXIS + extern Flags did_save_position; extern xyze_pos_t stored_position[SAVED_POSITIONS]; #endif