G60/G61 Position Save/Restore (MarlinFirmware#16557)

Author: Hans007a

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
 //

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

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"

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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#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<slot> - 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

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 <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#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<rate>  - Feedrate (optional) for the move back.
+ *   S<slot>  - 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

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

Marlin/src/gcode/gcode.h

@@ -67,6 +67,8 @@
  * G34  - Z Stepper automatic alignment using probe: I<iterations> T<accuracy> A<amplification> (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 @@ class GcodeSuite {
     static void G76();
   #endif
 
+  #if SAVED_POSITIONS
+    static void G60();
+    static void G61();
+  #endif
+
   #if ENABLED(GCODE_MOTION_MODES)
     static void G80();
   #endif

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

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<extruder> command.
 #if EXTRUDERS > 1
-  uint8_t active_extruder; // = 0
+  uint8_t active_extruder = 0; // = 0
 #endif
 
 #if ENABLED(LCD_SHOW_E_TOTAL)

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;