|
@@ -27,6 +27,10 @@
|
27
|
27
|
#include "../../../module/planner.h"
|
28
|
28
|
#include "../../gcode.h"
|
29
|
29
|
#include "../../../module/motion.h"
|
|
30
|
+#include "../../../module/planner.h"
|
|
31
|
+
|
|
32
|
+#define DEBUG_OUT ENABLED(SAVED_POSITIONS_DEBUG)
|
|
33
|
+#include "../../../core/debug_out.h"
|
30
|
34
|
|
31
|
35
|
/**
|
32
|
36
|
* G61: Return to saved position
|
|
@@ -34,11 +38,16 @@
|
34
|
38
|
* F<rate> - Feedrate (optional) for the move back.
|
35
|
39
|
* S<slot> - Slot # (0-based) to restore from (default 0).
|
36
|
40
|
* X Y Z - Axes to restore. At least one is required.
|
|
41
|
+ * E - Restore extruder position
|
|
42
|
+ *
|
|
43
|
+ * If XYZE are not given, default restore uses the smart blocking move.
|
37
|
44
|
*/
|
38
|
45
|
void GcodeSuite::G61(void) {
|
39
|
46
|
|
40
|
47
|
const uint8_t slot = parser.byteval('S');
|
41
|
48
|
|
|
49
|
+ #define SYNC_E(POINT) planner.set_e_position_mm((destination.e = current_position.e = (POINT)))
|
|
50
|
+
|
42
|
51
|
#if SAVED_POSITIONS < 256
|
43
|
52
|
if (slot >= SAVED_POSITIONS) {
|
44
|
53
|
SERIAL_ERROR_MSG(STR_INVALID_POS_SLOT STRINGIFY(SAVED_POSITIONS));
|
|
@@ -47,25 +56,37 @@ void GcodeSuite::G61(void) {
|
47
|
56
|
#endif
|
48
|
57
|
|
49
|
58
|
// No saved position? No axes being restored?
|
50
|
|
- if (!TEST(saved_slots[slot >> 3], slot & 0x07) || !parser.seen("XYZ")) return;
|
51
|
|
-
|
52
|
|
- SERIAL_ECHOPAIR(STR_RESTORING_POS " S", slot);
|
53
|
|
- LOOP_XYZ(i) {
|
54
|
|
- destination[i] = parser.seen(XYZ_CHAR(i))
|
55
|
|
- ? stored_position[slot][i] + parser.value_axis_units((AxisEnum)i)
|
56
|
|
- : current_position[i];
|
57
|
|
- SERIAL_CHAR(' ', XYZ_CHAR(i));
|
58
|
|
- SERIAL_ECHO_F(destination[i]);
|
59
|
|
- }
|
60
|
|
- SERIAL_EOL();
|
|
59
|
+ if (!TEST(saved_slots[slot >> 3], slot & 0x07)) return;
|
61
|
60
|
|
62
|
61
|
// Apply any given feedrate over 0.0
|
63
|
62
|
feedRate_t saved_feedrate = feedrate_mm_s;
|
64
|
63
|
const float fr = parser.linearval('F');
|
65
|
64
|
if (fr > 0.0) feedrate_mm_s = MMM_TO_MMS(fr);
|
66
|
65
|
|
67
|
|
- // Move to the saved position
|
68
|
|
- prepare_line_to_destination();
|
|
66
|
+ if (!parser.seen_axis()) {
|
|
67
|
+ DEBUG_ECHOLNPGM("Default position restore");
|
|
68
|
+ do_blocking_move_to(stored_position[slot], feedrate_mm_s);
|
|
69
|
+ SYNC_E(stored_position[slot].e);
|
|
70
|
+ }
|
|
71
|
+ else {
|
|
72
|
+ if (parser.seen("XYZ")) {
|
|
73
|
+ DEBUG_ECHOPAIR(STR_RESTORING_POS " S", slot);
|
|
74
|
+ LOOP_XYZ(i) {
|
|
75
|
+ destination[i] = parser.seen(XYZ_CHAR(i))
|
|
76
|
+ ? stored_position[slot][i] + parser.value_axis_units((AxisEnum)i)
|
|
77
|
+ : current_position[i];
|
|
78
|
+ DEBUG_CHAR(' ', XYZ_CHAR(i));
|
|
79
|
+ DEBUG_ECHO_F(destination[i]);
|
|
80
|
+ }
|
|
81
|
+ DEBUG_EOL();
|
|
82
|
+ // Move to the saved position
|
|
83
|
+ prepare_line_to_destination();
|
|
84
|
+ }
|
|
85
|
+ if (parser.seen_test('E')) {
|
|
86
|
+ DEBUG_ECHOLNPAIR(STR_RESTORING_POS " S", slot, " E", current_position.e, "=>", stored_position[slot].e);
|
|
87
|
+ SYNC_E(stored_position[slot].e);
|
|
88
|
+ }
|
|
89
|
+ }
|
69
|
90
|
|
70
|
91
|
feedrate_mm_s = saved_feedrate;
|
71
|
92
|
}
|