Skip to content

Commit 3b01bf4

Browse files
thinkyheadulendoalex
authored and
Tracy Spiva
committed
✨ Fixed-Time Motion with Input Shaping by Ulendo (MarlinFirmware#25394)
Co-authored-by: Ulendo Alex <alex@ulendo.io>
1 parent d20796c commit 3b01bf4

15 files changed

+1772
-50
lines changed

‎Marlin/Configuration_adv.h

+47-1
Original file line numberDiff line numberDiff line change
@@ -1096,7 +1096,51 @@
10961096

10971097
#endif
10981098

1099-
// @section motion
1099+
// @section motion control
1100+
1101+
/**
1102+
* Fixed-time-based Motion Control -- EXPERIMENTAL
1103+
* Enable/disable and set parameters with G-code M493.
1104+
*/
1105+
//#define FT_MOTION
1106+
#if ENABLED(FT_MOTION)
1107+
#define FTM_DEFAULT_MODE ftMotionMode_ENABLED // Default mode of fixed time control. (Enums in ft_types.h)
1108+
#define FTM_DEFAULT_DYNFREQ_MODE dynFreqMode_DISABLED // Default mode of dynamic frequency calculation. (Enums in ft_types.h)
1109+
#define FTM_SHAPING_DEFAULT_X_FREQ 37.0f // (Hz) Default peak frequency used by input shapers.
1110+
#define FTM_SHAPING_DEFAULT_Y_FREQ 37.0f // (Hz) Default peak frequency used by input shapers.
1111+
#define FTM_LINEAR_ADV_DEFAULT_ENA false // Default linear advance enable (true) or disable (false).
1112+
#define FTM_LINEAR_ADV_DEFAULT_K 0.0f // Default linear advance gain.
1113+
#define FTM_SHAPING_ZETA 0.1f // Zeta used by input shapers.
1114+
#define FTM_SHAPING_V_TOL 0.05f // Vibration tolerance used by EI input shapers.
1115+
1116+
/**
1117+
* Advanced configuration
1118+
*/
1119+
#define FTM_BATCH_SIZE 100 // Batch size for trajectory generation;
1120+
// half the window size for Ulendo FBS.
1121+
#define FTM_FS 1000 // (Hz) Frequency for trajectory generation. (1 / FTM_TS)
1122+
#define FTM_TS 0.001f // (s) Time step for trajectory generation. (1 / FTM_FS)
1123+
#define FTM_STEPPER_FS 20000 // (Hz) Frequency for stepper I/O update.
1124+
#define FTM_MIN_TICKS ((STEPPER_TIMER_RATE) / (FTM_STEPPER_FS)) // Minimum stepper ticks between steps.
1125+
#define FTM_MIN_SHAPE_FREQ 10 // Minimum shaping frequency.
1126+
#define FTM_ZMAX 100 // Maximum delays for shaping functions (even numbers only!).
1127+
// Calculate as:
1128+
// 1/2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for ZV.
1129+
// (FTM_FS / FTM_MIN_SHAPE_FREQ) for ZVD, MZV.
1130+
// 3/2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for 2HEI.
1131+
// 2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for 3HEI.
1132+
#define FTM_STEPS_PER_UNIT_TIME 20 // Interpolated stepper commands per unit time.
1133+
// Calculate as (FTM_STEPPER_FS / FTM_FS).
1134+
#define FTM_CTS_COMPARE_VAL 10 // Comparison value used in interpolation algorithm.
1135+
// Calculate as (FTM_STEPS_PER_UNIT_TIME / 2).
1136+
// These values may be configured to adjust duration of loop().
1137+
#define FTM_STEPS_PER_LOOP 60 // Number of stepper commands to generate each loop().
1138+
#define FTM_POINTS_PER_LOOP 100 // Number of trajectory points to generate each loop().
1139+
1140+
// This value may be configured to adjust duration to consume the command buffer.
1141+
// Try increasing this value if stepper motion is not smooth.
1142+
#define FTM_STEPPERCMD_BUFF_SIZE 1000 // Size of the stepper command buffers.
1143+
#endif
11001144

11011145
/**
11021146
* Input Shaping -- EXPERIMENTAL
@@ -1135,6 +1179,8 @@
11351179
//#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters.
11361180
#endif
11371181

1182+
// @section motion
1183+
11381184
#define AXIS_RELATIVE_MODES { false, false, false, false }
11391185

11401186
// Add a Duplicate option for well-separated conjoined nozzles

‎Marlin/src/MarlinCore.cpp

+7
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,9 @@
5050
#include "module/settings.h"
5151
#include "module/stepper.h"
5252
#include "module/temperature.h"
53+
#if ENABLED(FT_MOTION)
54+
#include "module/ft_motion.h"
55+
#endif
5356

5457
#include "gcode/gcode.h"
5558
#include "gcode/parser.h"
@@ -885,8 +888,12 @@ void idle(bool no_stepper_sleep/*=false*/) {
885888
// Update the LVGL interface
886889
TERN_(HAS_TFT_LVGL_UI, LV_TASK_HANDLER());
887890

891+
// Manage Fixed-time Motion Control
892+
TERN_(FT_MOTION, fxdTiCtrl.loop());
893+
888894
IDLE_DONE:
889895
TERN_(MARLIN_DEV_MODE, idle_depth--);
896+
890897
return;
891898
}
892899

‎Marlin/src/gcode/feature/ft_motion/M493.cpp

+282
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,282 @@
1+
/**
2+
* Marlin 3D Printer Firmware
3+
* Copyright (c) 2023 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
4+
*
5+
* Based on Sprinter and grbl.
6+
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
7+
*
8+
* This program is free software: you can redistribute it and/or modify
9+
* it under the terms of the GNU General Public License as published by
10+
* the Free Software Foundation, either version 3 of the License, or
11+
* (at your option) any later version.
12+
*
13+
* This program is distributed in the hope that it will be useful,
14+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
15+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16+
* GNU General Public License for more details.
17+
*
18+
* You should have received a copy of the GNU General Public License
19+
* along with this program. If not, see <https://www.gnu.org/licenses/>.
20+
*
21+
*/
22+
23+
#include "../../../inc/MarlinConfig.h"
24+
25+
#if ENABLED(FT_MOTION)
26+
27+
#include "../../gcode.h"
28+
#include "../../../module/ft_motion.h"
29+
30+
void say_shaping() {
31+
SERIAL_ECHO_TERNARY(fxdTiCtrl.cfg_mode, "Fixed time controller ", "en", "dis", "abled");
32+
if (fxdTiCtrl.cfg_mode == ftMotionMode_DISABLED || fxdTiCtrl.cfg_mode == ftMotionMode_ENABLED) {
33+
SERIAL_ECHOLNPGM(".");
34+
return;
35+
}
36+
#if HAS_X_AXIS
37+
SERIAL_ECHOPGM(" with ");
38+
switch (fxdTiCtrl.cfg_mode) {
39+
default: break;
40+
//case ftMotionMode_ULENDO_FBS: SERIAL_ECHOLNPGM("Ulendo FBS."); return;
41+
case ftMotionMode_ZV: SERIAL_ECHOLNPGM("ZV"); break;
42+
case ftMotionMode_ZVD: SERIAL_ECHOLNPGM("ZVD"); break;
43+
case ftMotionMode_EI: SERIAL_ECHOLNPGM("EI"); break;
44+
case ftMotionMode_2HEI: SERIAL_ECHOLNPGM("2 Hump EI"); break;
45+
case ftMotionMode_3HEI: SERIAL_ECHOLNPGM("3 Hump EI"); break;
46+
case ftMotionMode_MZV: SERIAL_ECHOLNPGM("MZV"); break;
47+
//case ftMotionMode_DISCTF: SERIAL_ECHOLNPGM("discrete transfer functions"); break;
48+
}
49+
SERIAL_ECHOLNPGM(" shaping.");
50+
#endif
51+
}
52+
53+
/**
54+
* M493: Set Fixed-time Motion Control parameters
55+
*
56+
* S<mode> Set the motion / shaping mode. Shaping requires an X axis, at the minimum.
57+
* 0: NORMAL
58+
* 1: FIXED-TIME
59+
* 10: ZV
60+
* 11: ZVD
61+
* 12: EI
62+
* 13: 2HEI
63+
* 14: 3HEI
64+
* 15: MZV
65+
*
66+
* P<bool> Enable (1) or Disable (0) Linear Advance pressure control
67+
*
68+
* K<gain> Set Linear Advance gain
69+
*
70+
* D<mode> Set Dynamic Frequency mode
71+
* 0: DISABLED
72+
* 1: Z-based (Requires a Z axis)
73+
* 2: Mass-based (Requires X and E axes)
74+
*
75+
* A<Hz> Set static/base frequency for the X axis
76+
* F<Hz> Set frequency scaling for the X axis
77+
*
78+
* B<Hz> Set static/base frequency for the Y axis
79+
* H<Hz> Set frequency scaling for the Y axis
80+
*/
81+
void GcodeSuite::M493() {
82+
// Parse 'S' mode parameter.
83+
if (parser.seenval('S')) {
84+
const ftMotionMode_t val = (ftMotionMode_t)parser.value_byte();
85+
switch (val) {
86+
case ftMotionMode_DISABLED:
87+
case ftMotionMode_ENABLED:
88+
#if HAS_X_AXIS
89+
case ftMotionMode_ZVD:
90+
case ftMotionMode_2HEI:
91+
case ftMotionMode_3HEI:
92+
case ftMotionMode_MZV:
93+
//case ftMotionMode_ULENDO_FBS:
94+
//case ftMotionMode_DISCTF:
95+
fxdTiCtrl.cfg_mode = val;
96+
say_shaping();
97+
break;
98+
#endif
99+
default:
100+
SERIAL_ECHOLNPGM("?Invalid control mode [M] value.");
101+
return;
102+
}
103+
104+
switch (val) {
105+
case ftMotionMode_ENABLED: fxdTiCtrl.reset(); break;
106+
#if HAS_X_AXIS
107+
case ftMotionMode_ZV:
108+
case ftMotionMode_ZVD:
109+
case ftMotionMode_EI:
110+
case ftMotionMode_2HEI:
111+
case ftMotionMode_3HEI:
112+
case ftMotionMode_MZV:
113+
fxdTiCtrl.updateShapingN(fxdTiCtrl.cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, fxdTiCtrl.cfg_baseFreq[1]));
114+
fxdTiCtrl.updateShapingA();
115+
fxdTiCtrl.reset();
116+
break;
117+
//case ftMotionMode_ULENDO_FBS:
118+
//case ftMotionMode_DISCTF:
119+
#endif
120+
default: break;
121+
}
122+
}
123+
124+
#if HAS_EXTRUDERS
125+
126+
// Pressure control (linear advance) parameter.
127+
if (parser.seen('P')) {
128+
const bool val = parser.value_bool();
129+
fxdTiCtrl.cfg_linearAdvEna = val;
130+
SERIAL_ECHO_TERNARY(val, "Pressure control: Linear Advance ", "en", "dis", "abled.\n");
131+
}
132+
133+
// Pressure control (linear advance) gain parameter.
134+
if (parser.seenval('K')) {
135+
const float val = parser.value_float();
136+
if (val >= 0.0f) {
137+
fxdTiCtrl.cfg_linearAdvK = val;
138+
SERIAL_ECHOPGM("Pressure control: Linear Advance gain set to: ");
139+
SERIAL_ECHO_F(val, 5);
140+
SERIAL_ECHOLNPGM(".");
141+
}
142+
else { // Value out of range.
143+
SERIAL_ECHOLNPGM("Pressure control: Linear Advance gain out of range.");
144+
}
145+
}
146+
147+
#endif // HAS_EXTRUDERS
148+
149+
#if HAS_Z_AXIS || HAS_EXTRUDERS
150+
151+
// Dynamic frequency mode parameter.
152+
if (parser.seenval('D')) {
153+
if (WITHIN(fxdTiCtrl.cfg_mode, 10U, 19U)) {
154+
const dynFreqMode_t val = dynFreqMode_t(parser.value_byte());
155+
switch (val) {
156+
case dynFreqMode_DISABLED:
157+
fxdTiCtrl.cfg_dynFreqMode = val;
158+
SERIAL_ECHOLNPGM("Dynamic frequency mode disabled.");
159+
break;
160+
#if HAS_Z_AXIS
161+
case dynFreqMode_Z_BASED:
162+
fxdTiCtrl.cfg_dynFreqMode = val;
163+
SERIAL_ECHOLNPGM("Z-based Dynamic Frequency Mode.");
164+
break;
165+
#endif
166+
#if HAS_EXTRUDERS
167+
case dynFreqMode_MASS_BASED:
168+
fxdTiCtrl.cfg_dynFreqMode = val;
169+
SERIAL_ECHOLNPGM("Mass-based Dynamic Frequency Mode.");
170+
break;
171+
#endif
172+
default:
173+
SERIAL_ECHOLNPGM("?Invalid Dynamic Frequency Mode [D] value.");
174+
break;
175+
}
176+
}
177+
else {
178+
SERIAL_ECHOLNPGM("Incompatible shaper for [D] Dynamic Frequency mode.");
179+
}
180+
}
181+
182+
#endif // HAS_Z_AXIS || HAS_EXTRUDERS
183+
184+
#if HAS_X_AXIS
185+
186+
// Parse frequency parameter (X axis).
187+
if (parser.seenval('A')) {
188+
if (WITHIN(fxdTiCtrl.cfg_mode, 10U, 19U)) {
189+
const float val = parser.value_float();
190+
const bool frequencyInRange = WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2);
191+
// TODO: Frequency minimum is dependent on the shaper used; the above check isn't always correct.
192+
if (frequencyInRange) {
193+
fxdTiCtrl.cfg_baseFreq[0] = val;
194+
fxdTiCtrl.updateShapingN(fxdTiCtrl.cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, fxdTiCtrl.cfg_baseFreq[1]));
195+
fxdTiCtrl.reset();
196+
if (fxdTiCtrl.cfg_dynFreqMode) { SERIAL_ECHOPGM("Compensator base dynamic frequency (X/A axis) set to:"); }
197+
else { SERIAL_ECHOPGM("Compensator static frequency (X/A axis) set to: "); }
198+
SERIAL_ECHO_F( fxdTiCtrl.cfg_baseFreq[0], 2 );
199+
SERIAL_ECHOLNPGM(".");
200+
}
201+
else { // Frequency out of range.
202+
SERIAL_ECHOLNPGM("Invalid [A] frequency value.");
203+
}
204+
}
205+
else { // Mode doesn't use frequency.
206+
SERIAL_ECHOLNPGM("Incompatible mode for [A] frequency.");
207+
}
208+
}
209+
210+
#if HAS_Z_AXIS || HAS_EXTRUDERS
211+
// Parse frequency scaling parameter (X axis).
212+
if (parser.seenval('F')) {
213+
const bool modeUsesDynFreq = (
214+
TERN0(HAS_Z_AXIS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_Z_BASED)
215+
|| TERN0(HAS_EXTRUDERS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_MASS_BASED)
216+
);
217+
218+
if (modeUsesDynFreq) {
219+
const float val = parser.value_float();
220+
fxdTiCtrl.cfg_dynFreqK[0] = val;
221+
SERIAL_ECHOPGM("Frequency scaling (X/A axis) set to: ");
222+
SERIAL_ECHO_F(fxdTiCtrl.cfg_dynFreqK[0], 8);
223+
SERIAL_ECHOLNPGM(".");
224+
}
225+
else {
226+
SERIAL_ECHOLNPGM("Incompatible mode for [F] frequency scaling.");
227+
}
228+
}
229+
#endif // HAS_Z_AXIS || HAS_EXTRUDERS
230+
231+
#endif // HAS_X_AXIS
232+
233+
#if HAS_Y_AXIS
234+
235+
// Parse frequency parameter (Y axis).
236+
if (parser.seenval('B')) {
237+
if (WITHIN(fxdTiCtrl.cfg_mode, 10U, 19U)) {
238+
const float val = parser.value_float();
239+
const bool frequencyInRange = WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2);
240+
if (frequencyInRange) {
241+
fxdTiCtrl.cfg_baseFreq[1] = val;
242+
fxdTiCtrl.updateShapingN(fxdTiCtrl.cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, fxdTiCtrl.cfg_baseFreq[1]));
243+
fxdTiCtrl.reset();
244+
if (fxdTiCtrl.cfg_dynFreqMode) { SERIAL_ECHOPGM("Compensator base dynamic frequency (Y/B axis) set to:"); }
245+
else { SERIAL_ECHOPGM("Compensator static frequency (Y/B axis) set to: "); }
246+
SERIAL_ECHO_F( fxdTiCtrl.cfg_baseFreq[1], 2 );
247+
SERIAL_ECHOLNPGM(".");
248+
}
249+
else { // Frequency out of range.
250+
SERIAL_ECHOLNPGM("Invalid frequency [B] value.");
251+
}
252+
}
253+
else { // Mode doesn't use frequency.
254+
SERIAL_ECHOLNPGM("Incompatible mode for [B] frequency.");
255+
}
256+
}
257+
258+
#if HAS_Z_AXIS || HAS_EXTRUDERS
259+
// Parse frequency scaling parameter (Y axis).
260+
if (parser.seenval('H')) {
261+
const bool modeUsesDynFreq = (
262+
TERN0(HAS_Z_AXIS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_Z_BASED)
263+
|| TERN0(HAS_EXTRUDERS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_MASS_BASED)
264+
);
265+
266+
if (modeUsesDynFreq) {
267+
const float val = parser.value_float();
268+
fxdTiCtrl.cfg_dynFreqK[1] = val;
269+
SERIAL_ECHOPGM("Frequency scaling (Y/B axis) set to: ");
270+
SERIAL_ECHO_F(val, 8);
271+
SERIAL_ECHOLNPGM(".");
272+
}
273+
else {
274+
SERIAL_ECHOLNPGM("Incompatible mode for [H] frequency scaling.");
275+
}
276+
}
277+
#endif // HAS_Z_AXIS || HAS_EXTRUDERS
278+
279+
#endif // HAS_Y_AXIS
280+
}
281+
282+
#endif // FT_MOTION

‎Marlin/src/gcode/gcode.cpp

+5-1
Original file line numberDiff line numberDiff line change
@@ -895,6 +895,10 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
895895
case 486: M486(); break; // M486: Identify and cancel objects
896896
#endif
897897

898+
#if ENABLED(FT_MOTION)
899+
case 493: M493(); break; // M493: Fixed-Time Motion control
900+
#endif
901+
898902
case 500: M500(); break; // M500: Store settings in EEPROM
899903
case 501: M501(); break; // M501: Read settings from EEPROM
900904
case 502: M502(); break; // M502: Revert to default settings
@@ -934,7 +938,7 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) {
934938
#endif
935939

936940
#if HAS_ZV_SHAPING
937-
case 593: M593(); break; // M593: Set Input Shaping parameters
941+
case 593: M593(); break; // M593: Input Shaping control
938942
#endif
939943

940944
#if ENABLED(ADVANCED_PAUSE_FEATURE)

‎Marlin/src/gcode/gcode.h

+4
Original file line numberDiff line numberDiff line change
@@ -1038,6 +1038,10 @@ class GcodeSuite {
10381038
static void M486();
10391039
#endif
10401040

1041+
#if ENABLED(FT_MOTION)
1042+
static void M493();
1043+
#endif
1044+
10411045
static void M500();
10421046
static void M501();
10431047
static void M502();

0 commit comments

Comments
 (0)