From e2eef1256a6feaceb8f4c7f02188e8d6d6d43519 Mon Sep 17 00:00:00 2001
From: Hans007a <32525735+Hans007a@users.noreply.github.com>
Date: Tue, 7 Jan 2020 20:52:19 +0100
Subject: [PATCH] G60/G61 Position Save/Restore (#16557)
---
Marlin/Configuration_adv.h | 5 ++
Marlin/src/HAL/HAL_STM32/timers.h | 10 ++--
Marlin/src/core/language.h | 4 ++
Marlin/src/gcode/feature/pause/G60.cpp | 59 +++++++++++++++++++++
Marlin/src/gcode/feature/pause/G61.cpp | 71 ++++++++++++++++++++++++++
Marlin/src/gcode/gcode.cpp | 22 +++++---
Marlin/src/gcode/gcode.h | 7 +++
Marlin/src/inc/Conditionals_adv.h | 5 ++
Marlin/src/module/motion.cpp | 8 ++-
Marlin/src/module/motion.h | 6 +++
10 files changed, 183 insertions(+), 14 deletions(-)
create mode 100644 Marlin/src/gcode/feature/pause/G60.cpp
create mode 100644 Marlin/src/gcode/feature/pause/G61.cpp
diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 51fcd1f4b..db5ad52ab 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -1576,6 +1576,11 @@
// @section extras
+//
+// G60/G61 Position Save and Return
+//
+//#define SAVED_POSITIONS 1 // Each saved position slot costs 12 bytes
+
//
// G2/G3 Arc Support
//
diff --git a/Marlin/src/HAL/HAL_STM32/timers.h b/Marlin/src/HAL/HAL_STM32/timers.h
index aa85836bd..f1dce173e 100644
--- a/Marlin/src/HAL/HAL_STM32/timers.h
+++ b/Marlin/src/HAL/HAL_STM32/timers.h
@@ -61,8 +61,6 @@
#define HAL_TIMER_RATE (F_CPU / 2) // frequency of timer peripherals
- // STM32F401 only has timers 1-5 & 9-11 with timers 4 & 5 usually assigned to TIMER_SERVO and TIMER_TONE
-
#ifndef STEP_TIMER
#define STEP_TIMER 9
#endif
@@ -76,19 +74,19 @@
#define HAL_TIMER_RATE (F_CPU / 2) // frequency of timer peripherals
#ifndef STEP_TIMER
- #define STEP_TIMER 6
+ #define STEP_TIMER 6 // STM32F401 has no TIM6, TIM7, or TIM8
#endif
#ifndef TEMP_TIMER
- #define TEMP_TIMER 14
+ #define TEMP_TIMER 14 // TIM7 is consumed by Software Serial if used.
#endif
#elif defined(STM32F7xx)
- #define HAL_TIMER_RATE (F_CPU/2) // frequency of timer peripherals
+ #define HAL_TIMER_RATE (F_CPU / 2) // frequency of timer peripherals
#ifndef STEP_TIMER
- #define STEP_TIMER 6
+ #define STEP_TIMER 6 // the RIGHT timer!
#endif
#ifndef TEMP_TIMER
diff --git a/Marlin/src/core/language.h b/Marlin/src/core/language.h
index fe279bae8..ecafd49a8 100644
--- a/Marlin/src/core/language.h
+++ b/Marlin/src/core/language.h
@@ -222,6 +222,10 @@
#define MSG_SOFT_MIN " Min: "
#define MSG_SOFT_MAX " Max: "
+#define MSG_SAVED_POS "Position saved"
+#define MSG_RESTORING_POS "Restoring position"
+#define MSG_INVALID_POS_SLOT "Invalid slot. Total: "
+
#define MSG_SD_CANT_OPEN_SUBDIR "Cannot open subdir "
#define MSG_SD_INIT_FAIL "SD init fail"
#define MSG_SD_VOL_INIT_FAIL "volume.init failed"
diff --git a/Marlin/src/gcode/feature/pause/G60.cpp b/Marlin/src/gcode/feature/pause/G60.cpp
new file mode 100644
index 000000000..94b73cd7d
--- /dev/null
+++ b/Marlin/src/gcode/feature/pause/G60.cpp
@@ -0,0 +1,59 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2019 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 .
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if SAVED_POSITIONS
+
+#include "../../../core/language.h"
+#include "../../gcode.h"
+#include "../../../module/motion.h"
+
+#define DEBUG_OUT ENABLED(SAVED_POSITIONS_DEBUG)
+#include "../../../core/debug_out.h"
+
+/**
+ * G60: Save current position
+ *
+ * S - Memory slot # (0-based) to save into (default 0)
+ */
+void GcodeSuite::G60() {
+ const uint8_t slot = parser.byteval('S');
+
+ if (slot >= SAVED_POSITIONS) {
+ SERIAL_ERROR_MSG(MSG_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS));
+ return;
+ }
+
+ stored_position[slot] = current_position;
+ SBI(saved_slots, slot);
+
+ #if ENABLED(SAVED_POSITIONS_DEBUG)
+ const xyze_pos_t &pos = stored_position[slot];
+ DEBUG_ECHOPAIR_F(MSG_SAVED_POS " S", slot);
+ DEBUG_ECHOPAIR_F(" : X", pos.x);
+ DEBUG_ECHOPAIR_F_P(SP_Y_STR, pos.y);
+ DEBUG_ECHOLNPAIR_F_P(SP_Z_STR, pos.z);
+ #endif
+}
+
+#endif // SAVED_POSITIONS
diff --git a/Marlin/src/gcode/feature/pause/G61.cpp b/Marlin/src/gcode/feature/pause/G61.cpp
new file mode 100644
index 000000000..5d854dfab
--- /dev/null
+++ b/Marlin/src/gcode/feature/pause/G61.cpp
@@ -0,0 +1,71 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2019 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 .
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if SAVED_POSITIONS
+
+#include "../../../core/language.h"
+#include "../../module/planner.h"
+#include "../../gcode.h"
+#include "../../../module/motion.h"
+
+/**
+ * G61: Return to saved position
+ *
+ * F - Feedrate (optional) for the move back.
+ * S - Slot # (0-based) to restore from (default 0).
+ * X Y Z - Axes to restore. At least one is required.
+ */
+void GcodeSuite::G61(void) {
+
+ const uint8_t slot = parser.byteval('S');
+
+ #if SAVED_POSITIONS < 256
+ if (slot >= SAVED_POSITIONS) {
+ SERIAL_ERROR_MSG(MSG_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS));
+ return;
+ }
+ #endif
+
+ // No saved position? No axes being restored?
+ if (!TEST(saved_slots, slot) || !parser.seen("XYZ")) return;
+
+ // Apply any given feedrate over 0.0
+ const float fr = parser.linearval('F');
+ if (fr > 0.0) feedrate_mm_s = MMM_TO_MMS(fr);
+
+ SERIAL_ECHOPAIR(MSG_RESTORING_POS " S", int(slot));
+ LOOP_XYZ(i) {
+ destination[i] = parser.seen(axis_codes[i])
+ ? stored_position[slot][i] + parser.value_axis_units((AxisEnum)i)
+ : current_position[i];
+ SERIAL_CHAR(' ', axis_codes[i]);
+ SERIAL_ECHO_F(destination[i]);
+ }
+ SERIAL_EOL();
+
+ // Move to the saved position
+ prepare_move_to_destination();
+}
+
+#endif // SAVED_POSITIONS
diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp
index 7ff86f314..955e62e80 100644
--- a/Marlin/src/gcode/gcode.cpp
+++ b/Marlin/src/gcode/gcode.cpp
@@ -314,19 +314,27 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
#endif
#if ENABLED(CNC_COORDINATE_SYSTEMS)
- case 53: G53(); break;
- case 54: G54(); break;
- case 55: G55(); break;
- case 56: G56(); break;
- case 57: G57(); break;
- case 58: G58(); break;
- case 59: G59(); break;
+ case 53: G53(); break; // G53: (prefix) Apply native workspace
+ case 54: G54(); break; // G54: Switch to Workspace 1
+ case 55: G55(); break; // G55: Switch to Workspace 2
+ case 56: G56(); break; // G56: Switch to Workspace 3
+ case 57: G57(); break; // G57: Switch to Workspace 4
+ case 58: G58(); break; // G58: Switch to Workspace 5
+ case 59: G59(); break; // G59.0 - G59.3: Switch to Workspace 6-9
+ #endif
+
+ #if SAVED_POSITIONS
+ case 60: G60(); break; // G60: save current position
+ case 61: G61(); break; // G61: Apply/restore saved coordinates.
#endif
#if ENABLED(PROBE_TEMP_COMPENSATION)
case 76: G76(); break; // G76: Calibrate first layer compensation values
#endif
+ case 60: G60(); break; // G60: save current position
+ case 61: G61(); break; // G61: Apply/restore saved coordinates.
+
#if ENABLED(GCODE_MOTION_MODES)
case 80: G80(); break; // G80: Reset the current motion mode
#endif
diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h
index f7111c6ee..5bbc5e862 100644
--- a/Marlin/src/gcode/gcode.h
+++ b/Marlin/src/gcode/gcode.h
@@ -67,6 +67,8 @@
* G34 - Z Stepper automatic alignment using probe: I T A (Requires Z_STEPPER_AUTO_ALIGN)
* G38 - Probe in any direction using the Z_MIN_PROBE (Requires G38_PROBE_TARGET)
* G42 - Coordinated move to a mesh point (Requires MESH_BED_LEVELING, AUTO_BED_LEVELING_BLINEAR, or AUTO_BED_LEVELING_UBL)
+ * G60 - Save current position. (Requires SAVED_POSITIONS)
+ * G61 - Apply/restore saved coordinates. (Requires SAVED_POSITIONS)
* G76 - Calibrate first layer temperature offsets. (Requires PROBE_TEMP_COMPENSATION)
* G80 - Cancel current motion mode (Requires GCODE_MOTION_MODES)
* G90 - Use Absolute Coordinates
@@ -471,6 +473,11 @@ private:
static void G76();
#endif
+ #if SAVED_POSITIONS
+ static void G60();
+ static void G61();
+ #endif
+
#if ENABLED(GCODE_MOTION_MODES)
static void G80();
#endif
diff --git a/Marlin/src/inc/Conditionals_adv.h b/Marlin/src/inc/Conditionals_adv.h
index 00be08433..7ac2d167a 100644
--- a/Marlin/src/inc/Conditionals_adv.h
+++ b/Marlin/src/inc/Conditionals_adv.h
@@ -142,3 +142,8 @@
#if ENABLED(JOYSTICK)
#define POLL_JOG
#endif
+
+// G60/G61 Position Save
+#if SAVED_POSITIONS > 256
+ #error "SAVED_POSITIONS must be an integer from 0 to 256."
+#endif
diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp
index 9d0422c4a..6dbf940ad 100644
--- a/Marlin/src/module/motion.cpp
+++ b/Marlin/src/module/motion.cpp
@@ -109,9 +109,15 @@ xyze_pos_t current_position = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
*/
xyze_pos_t destination; // {0}
+// G60/G61 Position Save and Return
+#if SAVED_POSITIONS
+ uint8_t saved_slots;
+ xyz_pos_t stored_position[SAVED_POSITIONS];
+#endif
+
// The active extruder (tool). Set with T command.
#if EXTRUDERS > 1
- uint8_t active_extruder; // = 0
+ uint8_t active_extruder = 0; // = 0
#endif
#if ENABLED(LCD_SHOW_E_TOTAL)
diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h
index 43ae2b230..a30573e79 100644
--- a/Marlin/src/module/motion.h
+++ b/Marlin/src/module/motion.h
@@ -65,6 +65,12 @@ extern bool relative_mode;
extern xyze_pos_t current_position, // High-level current tool position
destination; // Destination for a move
+// G60/G61 Position Save and Return
+#if SAVED_POSITIONS
+ extern uint8_t saved_slots;
+ extern xyz_pos_t stored_position[SAVED_POSITIONS];
+#endif
+
// Scratch space for a cartesian result
extern xyz_pos_t cartes;