mirror of
https://github.com/MarlinFirmware/Marlin.git
synced 2024-11-29 23:07:42 +00:00
👷 FT Motion refactor, minor fix
This commit is contained in:
Scott Lahteine
parent
295f50379f
commit
2d609487ac
@ -98,18 +98,18 @@ void say_shaping() {
|
||||
|
||||
#if HAS_X_AXIS
|
||||
SERIAL_ECHO_TERNARY(dynamic, AXIS_0_NAME " ", "base dynamic", "static", " shaper frequency: ");
|
||||
SERIAL_ECHO(p_float_t(ftMotion.cfg.baseFreq[X_AXIS], 2), F("Hz"));
|
||||
SERIAL_ECHO(p_float_t(ftMotion.cfg.baseFreq.x, 2), F("Hz"));
|
||||
#if HAS_DYNAMIC_FREQ
|
||||
if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(ftMotion.cfg.dynFreqK[X_AXIS], 2), F("Hz/"), z_based ? F("mm") : F("g"));
|
||||
if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(ftMotion.cfg.dynFreqK.x, 2), F("Hz/"), z_based ? F("mm") : F("g"));
|
||||
#endif
|
||||
SERIAL_EOL();
|
||||
#endif
|
||||
|
||||
#if HAS_Y_AXIS
|
||||
SERIAL_ECHO_TERNARY(dynamic, AXIS_1_NAME " ", "base dynamic", "static", " shaper frequency: ");
|
||||
SERIAL_ECHO(p_float_t(ftMotion.cfg.baseFreq[Y_AXIS], 2), F(" Hz"));
|
||||
SERIAL_ECHO(p_float_t(ftMotion.cfg.baseFreq.y, 2), F(" Hz"));
|
||||
#if HAS_DYNAMIC_FREQ
|
||||
if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(ftMotion.cfg.dynFreqK[Y_AXIS], 2), F("Hz/"), z_based ? F("mm") : F("g"));
|
||||
if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(ftMotion.cfg.dynFreqK.y, 2), F("Hz/"), z_based ? F("mm") : F("g"));
|
||||
#endif
|
||||
SERIAL_EOL();
|
||||
#endif
|
||||
@ -131,17 +131,17 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) {
|
||||
const ft_config_t &c = ftMotion.cfg;
|
||||
SERIAL_ECHOPGM(" M493 S", c.active);
|
||||
#if HAS_X_AXIS
|
||||
SERIAL_ECHOPGM(" A", c.baseFreq[X_AXIS]);
|
||||
SERIAL_ECHOPGM(" A", c.baseFreq.x);
|
||||
#if HAS_Y_AXIS
|
||||
SERIAL_ECHOPGM(" B", c.baseFreq[Y_AXIS]);
|
||||
SERIAL_ECHOPGM(" B", c.baseFreq.y);
|
||||
#endif
|
||||
#endif
|
||||
#if HAS_DYNAMIC_FREQ
|
||||
SERIAL_ECHOPGM(" D", c.dynFreqMode);
|
||||
#if HAS_X_AXIS
|
||||
SERIAL_ECHOPGM(" F", c.dynFreqK[X_AXIS]);
|
||||
SERIAL_ECHOPGM(" F", c.dynFreqK.x);
|
||||
#if HAS_Y_AXIS
|
||||
SERIAL_ECHOPGM(" H", c.dynFreqK[Y_AXIS]);
|
||||
SERIAL_ECHOPGM(" H", c.dynFreqK.y);
|
||||
#endif
|
||||
#endif
|
||||
#endif
|
||||
@ -308,7 +308,7 @@ void GcodeSuite::M493() {
|
||||
const float val = parser.value_float();
|
||||
// TODO: Frequency minimum is dependent on the shaper used; the above check isn't always correct.
|
||||
if (WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2)) {
|
||||
ftMotion.cfg.baseFreq[X_AXIS] = val;
|
||||
ftMotion.cfg.baseFreq.x = val;
|
||||
flag.update = flag.reset_ft = flag.report_h = true;
|
||||
}
|
||||
else // Frequency out of range.
|
||||
@ -322,7 +322,7 @@ void GcodeSuite::M493() {
|
||||
// Parse frequency scaling parameter (X axis).
|
||||
if (parser.seenval('F')) {
|
||||
if (modeUsesDynFreq) {
|
||||
ftMotion.cfg.dynFreqK[X_AXIS] = parser.value_float();
|
||||
ftMotion.cfg.dynFreqK.x = parser.value_float();
|
||||
flag.report_h = true;
|
||||
}
|
||||
else
|
||||
@ -369,7 +369,7 @@ void GcodeSuite::M493() {
|
||||
if (AXIS_HAS_SHAPER(Y)) {
|
||||
const float val = parser.value_float();
|
||||
if (WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2)) {
|
||||
ftMotion.cfg.baseFreq[Y_AXIS] = val;
|
||||
ftMotion.cfg.baseFreq.y = val;
|
||||
flag.update = flag.reset_ft = flag.report_h = true;
|
||||
}
|
||||
else // Frequency out of range.
|
||||
@ -383,7 +383,7 @@ void GcodeSuite::M493() {
|
||||
// Parse frequency scaling parameter (Y axis).
|
||||
if (parser.seenval('H')) {
|
||||
if (modeUsesDynFreq) {
|
||||
ftMotion.cfg.dynFreqK[Y_AXIS] = parser.value_float();
|
||||
ftMotion.cfg.dynFreqK.y = parser.value_float();
|
||||
flag.report_h = true;
|
||||
}
|
||||
else
|
||||
|
||||
@ -4381,7 +4381,7 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive."
|
||||
#error "FT_MOTION requires FTM_UNIFIED_BWS to be enabled because FBS is not yet implemented."
|
||||
#endif
|
||||
#if !HAS_X_AXIS
|
||||
static_assert(FTM_DEFAULT_X_COMPENSATOR != ftMotionShaper_NONE, "Without any linear axes FTM_DEFAULT_X_COMPENSATOR must be ftMotionShaper_NONE.");
|
||||
static_assert(FTM_DEFAULT_SHAPER_X != ftMotionShaper_NONE, "Without any linear axes FTM_DEFAULT_SHAPER_X must be ftMotionShaper_NONE.");
|
||||
#endif
|
||||
#if HAS_DYNAMIC_FREQ_MM
|
||||
static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_Z_BASED, "dynFreqMode_Z_BASED requires a Z axis.");
|
||||
|
||||
@ -358,7 +358,7 @@ void menu_move() {
|
||||
}
|
||||
|
||||
inline void menu_ftm_cmpn_x() {
|
||||
const ftMotionShaper_t shaper = ftMotion.cfg.shaper[X_AXIS];
|
||||
const ftMotionShaper_t shaper = ftMotion.cfg.shaper.x;
|
||||
START_MENU();
|
||||
BACK_ITEM(MSG_FIXED_TIME_MOTION);
|
||||
|
||||
@ -376,7 +376,7 @@ void menu_move() {
|
||||
}
|
||||
|
||||
inline void menu_ftm_cmpn_y() {
|
||||
const ftMotionShaper_t shaper = ftMotion.cfg.shaper[Y_AXIS];
|
||||
const ftMotionShaper_t shaper = ftMotion.cfg.shaper.y;
|
||||
START_MENU();
|
||||
BACK_ITEM(MSG_FIXED_TIME_MOTION);
|
||||
|
||||
@ -442,10 +442,10 @@ void menu_move() {
|
||||
MENU_ITEM_ADDON_START_RJ(5); lcd_put_u8str(shaper_name[X_AXIS]); MENU_ITEM_ADDON_END();
|
||||
|
||||
if (AXIS_HAS_SHAPER(X)) {
|
||||
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq[X_AXIS], FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params);
|
||||
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_ZETA_N, &c.zeta[0], 0.0f, 1.0f, ftMotion.update_shaping_params);
|
||||
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq.x, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params);
|
||||
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_ZETA_N, &c.zeta.x, 0.0f, 1.0f, ftMotion.update_shaping_params);
|
||||
if (AXIS_HAS_EISHAPER(X))
|
||||
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_VTOL_N, &c.vtol[0], 0.0f, 1.0f, ftMotion.update_shaping_params);
|
||||
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_VTOL_N, &c.vtol.x, 0.0f, 1.0f, ftMotion.update_shaping_params);
|
||||
}
|
||||
#endif
|
||||
#if HAS_Y_AXIS
|
||||
@ -453,10 +453,10 @@ void menu_move() {
|
||||
MENU_ITEM_ADDON_START_RJ(5); lcd_put_u8str(shaper_name[Y_AXIS]); MENU_ITEM_ADDON_END();
|
||||
|
||||
if (AXIS_HAS_SHAPER(Y)) {
|
||||
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq[Y_AXIS], FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params);
|
||||
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_ZETA_N, &c.zeta[1], 0.0f, 1.0f, ftMotion.update_shaping_params);
|
||||
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq.y, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params);
|
||||
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_ZETA_N, &c.zeta.y, 0.0f, 1.0f, ftMotion.update_shaping_params);
|
||||
if (AXIS_HAS_EISHAPER(Y))
|
||||
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_VTOL_N, &c.vtol[1], 0.0f, 1.0f, ftMotion.update_shaping_params);
|
||||
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_VTOL_N, &c.vtol.y, 0.0f, 1.0f, ftMotion.update_shaping_params);
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -465,10 +465,10 @@ void menu_move() {
|
||||
MENU_ITEM_ADDON_START_RJ(11); lcd_put_u8str(dmode); MENU_ITEM_ADDON_END();
|
||||
if (c.dynFreqMode != dynFreqMode_DISABLED) {
|
||||
#if HAS_X_AXIS
|
||||
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_DFREQ_K_N, &c.dynFreqK[X_AXIS], 0.0f, 20.0f);
|
||||
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_DFREQ_K_N, &c.dynFreqK.x, 0.0f, 20.0f);
|
||||
#endif
|
||||
#if HAS_Y_AXIS
|
||||
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_DFREQ_K_N, &c.dynFreqK[Y_AXIS], 0.0f, 20.0f);
|
||||
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_DFREQ_K_N, &c.dynFreqK.y, 0.0f, 20.0f);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
@ -492,8 +492,6 @@ void menu_move() {
|
||||
MString<20> dmode = get_dyn_freq_mode_name();
|
||||
#endif
|
||||
|
||||
ft_config_t &c = ftMotion.cfg;
|
||||
|
||||
START_MENU();
|
||||
|
||||
#if HAS_X_AXIS
|
||||
@ -514,7 +512,6 @@ void menu_move() {
|
||||
#endif
|
||||
|
||||
END_MENU();
|
||||
|
||||
}
|
||||
|
||||
#endif // FT_MOTION_MENU
|
||||
|
||||
@ -134,7 +134,7 @@ void FTMotion::loop() {
|
||||
}
|
||||
|
||||
while (!blockProcRdy && (stepper.current_block = planner.get_current_block())) {
|
||||
if (stepper.current_block->is_sync()) { // Sync block?
|
||||
if (stepper.current_block->is_sync()) { // Sync block?
|
||||
if (stepper.current_block->is_sync_pos()) // Position sync? Set the position.
|
||||
stepper._set_position(stepper.current_block->position);
|
||||
discard_planner_block_protected();
|
||||
@ -166,7 +166,7 @@ void FTMotion::loop() {
|
||||
discard_planner_block_protected();
|
||||
|
||||
// Check if the block needs to be runout:
|
||||
if (!batchRdy && !planner.movesplanned()){
|
||||
if (!batchRdy && !planner.movesplanned()) {
|
||||
runoutBlock();
|
||||
makeVector(); // Do an additional makeVector call to guarantee batchRdy set this loop.
|
||||
}
|
||||
@ -196,7 +196,7 @@ void FTMotion::loop() {
|
||||
batchRdy = false; // Clear so makeVector() can resume generating points.
|
||||
}
|
||||
|
||||
// Interpolation.
|
||||
// Interpolation (generation of step commands from fixed time trajectory).
|
||||
while (batchRdyForInterp
|
||||
&& (stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME))) {
|
||||
convertToSteps(interpIdx);
|
||||
@ -350,14 +350,14 @@ void FTMotion::loop() {
|
||||
void FTMotion::update_shaping_params() {
|
||||
#if HAS_X_AXIS
|
||||
if ((shaping.x.ena = AXIS_HAS_SHAPER(X))) {
|
||||
shaping.x.set_axis_shaping_A(cfg.shaper[X_AXIS], cfg.zeta[X_AXIS], cfg.vtol[X_AXIS]);
|
||||
shaping.x.set_axis_shaping_N(cfg.shaper[X_AXIS], cfg.baseFreq[X_AXIS], cfg.zeta[X_AXIS]);
|
||||
shaping.x.set_axis_shaping_A(cfg.shaper.x, cfg.zeta.x, cfg.vtol.x);
|
||||
shaping.x.set_axis_shaping_N(cfg.shaper.x, cfg.baseFreq.x, cfg.zeta.x);
|
||||
}
|
||||
#endif
|
||||
#if HAS_Y_AXIS
|
||||
if ((shaping.y.ena = AXIS_HAS_SHAPER(Y))) {
|
||||
shaping.y.set_axis_shaping_A(cfg.shaper[Y_AXIS], cfg.zeta[Y_AXIS], cfg.vtol[Y_AXIS]);
|
||||
shaping.y.set_axis_shaping_N(cfg.shaper[Y_AXIS], cfg.baseFreq[Y_AXIS], cfg.zeta[Y_AXIS]);
|
||||
shaping.y.set_axis_shaping_A(cfg.shaper.y, cfg.zeta.y, cfg.vtol.y);
|
||||
shaping.y.set_axis_shaping_N(cfg.shaper.y, cfg.baseFreq.y, cfg.zeta.y);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
@ -407,27 +407,29 @@ void FTMotion::discard_planner_block_protected() {
|
||||
}
|
||||
}
|
||||
|
||||
// Sets up a pseudo block to allow motion to settle buffers to empty. This is
|
||||
// called when the planner has only one block left. The buffers will be filled
|
||||
// with the last commanded position by setting the startPosn block variable to
|
||||
// the last position of the previous block and all ratios to zero such that no
|
||||
// axes' positions are incremented.
|
||||
/**
|
||||
* Set up a pseudo block to allow motion to settle and buffers to empty.
|
||||
* Called when the planner has one block left. The buffers will be filled
|
||||
* with the last commanded position by setting the startPosn block variable to
|
||||
* the last position of the previous block and all ratios to zero such that no
|
||||
* axes' positions are incremented.
|
||||
*/
|
||||
void FTMotion::runoutBlock() {
|
||||
|
||||
startPosn = endPosn_prevBlock;
|
||||
ratio.reset();
|
||||
|
||||
int32_t n_to_fill_batch = FTM_WINDOW_SIZE - makeVector_batchIdx;
|
||||
const int32_t n_to_fill_batch = (FTM_WINDOW_SIZE) - makeVector_batchIdx;
|
||||
|
||||
// This line is to be modified for FBS use; do not optimize out.
|
||||
int32_t n_to_settle_cmpnstr = (TERN_(HAS_X_AXIS, shaping.x.ena) || TERN_(HAS_Y_AXIS, shaping.y.ena )) ? FTM_ZMAX : 0;
|
||||
// This line or function is to be modified for FBS use; do not optimize out.
|
||||
const int32_t n_to_settle_shaper = num_samples_shaper_settle();
|
||||
|
||||
int32_t n_to_fill_batch_after_settling = (n_to_settle_cmpnstr > n_to_fill_batch) ?
|
||||
FTM_BATCH_SIZE - ((n_to_settle_cmpnstr - n_to_fill_batch) % FTM_BATCH_SIZE) : n_to_fill_batch - n_to_settle_cmpnstr;
|
||||
const int32_t n_diff = n_to_settle_shaper - n_to_fill_batch,
|
||||
n_to_fill_batch_after_settling = n_diff > 0 ? (FTM_BATCH_SIZE) - (n_diff % (FTM_BATCH_SIZE)) : -n_diff;
|
||||
|
||||
int32_t n_to_settle_and_fill_batch = n_to_settle_cmpnstr + n_to_fill_batch_after_settling;
|
||||
const int32_t n_to_settle_and_fill_batch = n_to_settle_shaper + n_to_fill_batch_after_settling;
|
||||
|
||||
max_intervals = PROP_BATCHES * FTM_BATCH_SIZE + n_to_settle_and_fill_batch;
|
||||
max_intervals = (PROP_BATCHES) * (FTM_BATCH_SIZE) + n_to_settle_and_fill_batch;
|
||||
|
||||
blockProcRdy = true;
|
||||
}
|
||||
@ -571,13 +573,13 @@ void FTMotion::makeVector() {
|
||||
accel_k = decel_P; // (mm/s^2) Acceleration K factor from Decel phase
|
||||
}
|
||||
|
||||
#define _FTM_TRAJ(A) traj.A[makeVector_batchIdx] = startPosn.A + ratio.A * dist;
|
||||
LOGICAL_AXIS_MAP_LC(_FTM_TRAJ);
|
||||
#define _SET_TRAJ(q) traj.q[makeVector_batchIdx] = startPosn.q + ratio.q * dist;
|
||||
LOGICAL_AXIS_MAP_LC(_SET_TRAJ);
|
||||
|
||||
#if HAS_EXTRUDERS
|
||||
if (cfg.linearAdvEna) {
|
||||
float dedt_adj = (traj.e[makeVector_batchIdx] - e_raw_z1) * (FTM_FS);
|
||||
if (ratio.e > 0.0f) dedt_adj += accel_k * cfg.linearAdvK * 0.0001f;
|
||||
if (ratio.e > 0.0f) dedt_adj += accel_k * cfg.linearAdvK;
|
||||
|
||||
e_raw_z1 = traj.e[makeVector_batchIdx];
|
||||
e_advanced_z1 += dedt_adj * (FTM_TS);
|
||||
@ -590,18 +592,21 @@ void FTMotion::makeVector() {
|
||||
switch (cfg.dynFreqMode) {
|
||||
|
||||
#if HAS_DYNAMIC_FREQ_MM
|
||||
case dynFreqMode_Z_BASED:
|
||||
if (traj.z[makeVector_batchIdx] != 0.0f) { // Only update if Z changed.
|
||||
case dynFreqMode_Z_BASED: {
|
||||
static float oldz = 0.0f;
|
||||
const float z = traj.z[makeVector_batchIdx];
|
||||
if (z != oldz) { // Only update if Z changed.
|
||||
oldz = z;
|
||||
#if HAS_X_AXIS
|
||||
const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.z[makeVector_batchIdx];
|
||||
shaping.x.set_axis_shaping_N(cfg.shaper[X_AXIS], _MAX(xf, FTM_MIN_SHAPE_FREQ), cfg.zeta[X_AXIS]);
|
||||
const float xf = cfg.baseFreq.x + cfg.dynFreqK.x * z;
|
||||
shaping.x.set_axis_shaping_N(cfg.shaper.x, _MAX(xf, FTM_MIN_SHAPE_FREQ), cfg.zeta.x);
|
||||
#endif
|
||||
#if HAS_Y_AXIS
|
||||
const float yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.z[makeVector_batchIdx];
|
||||
shaping.y.set_axis_shaping_N(cfg.shaper[Y_AXIS], _MAX(yf, FTM_MIN_SHAPE_FREQ), cfg.zeta[Y_AXIS]);
|
||||
const float yf = cfg.baseFreq.y + cfg.dynFreqK.y * z;
|
||||
shaping.y.set_axis_shaping_N(cfg.shaper.y, _MAX(yf, FTM_MIN_SHAPE_FREQ), cfg.zeta.y);
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
} break;
|
||||
#endif
|
||||
|
||||
#if HAS_DYNAMIC_FREQ_G
|
||||
@ -609,10 +614,10 @@ void FTMotion::makeVector() {
|
||||
// Update constantly. The optimization done for Z value makes
|
||||
// less sense for E, as E is expected to constantly change.
|
||||
#if HAS_X_AXIS
|
||||
shaping.x.set_axis_shaping_N(cfg.shaper[X_AXIS], cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.e[makeVector_batchIdx], cfg.zeta[X_AXIS]);
|
||||
shaping.x.set_axis_shaping_N(cfg.shaper.x, cfg.baseFreq.x + cfg.dynFreqK.x * traj.e[makeVector_batchIdx], cfg.zeta.x);
|
||||
#endif
|
||||
#if HAS_Y_AXIS
|
||||
shaping.y.set_axis_shaping_N(cfg.shaper[Y_AXIS], cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.e[makeVector_batchIdx], cfg.zeta[Y_AXIS]);
|
||||
shaping.y.set_axis_shaping_N(cfg.shaper.y, cfg.baseFreq.y + cfg.dynFreqK.y * traj.e[makeVector_batchIdx], cfg.zeta.y);
|
||||
#endif
|
||||
break;
|
||||
#endif
|
||||
@ -722,7 +727,7 @@ void FTMotion::convertToSteps(const uint32_t idx) {
|
||||
err_P += delta;
|
||||
|
||||
// Set up step/dir bits for all axes
|
||||
#define _COMMAND_RUN(AXIS) command_set[_AXIS(AXIS)](err_P[_AXIS(AXIS)], steps[_AXIS(AXIS)], cmd, _BV(FT_BIT_DIR_##AXIS), _BV(FT_BIT_STEP_##AXIS));
|
||||
#define _COMMAND_RUN(A) command_set[_AXIS(A)](err_P.A, steps.A, cmd, _BV(FT_BIT_DIR_##A), _BV(FT_BIT_STEP_##A));
|
||||
LOGICAL_AXIS_MAP(_COMMAND_RUN);
|
||||
|
||||
// Next circular buffer index
|
||||
|
||||
@ -36,25 +36,23 @@
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#define NUM_AXES_SHAPED TERN(HAS_Y_AXIS, 2, 1)
|
||||
|
||||
typedef struct FTConfig {
|
||||
bool active = ENABLED(FTM_IS_DEFAULT_MOTION); // Active (else standard motion)
|
||||
|
||||
#if HAS_X_AXIS
|
||||
ftMotionShaper_t shaper[NUM_AXES_SHAPED] = // Shaper type
|
||||
{ FTM_DEFAULT_SHAPER_X OPTARG(HAS_Y_AXIS, FTM_DEFAULT_SHAPER_Y) };
|
||||
float baseFreq[NUM_AXES_SHAPED] = // Base frequency. [Hz]
|
||||
{ FTM_SHAPING_DEFAULT_X_FREQ OPTARG(HAS_Y_AXIS, FTM_SHAPING_DEFAULT_Y_FREQ) };
|
||||
float zeta[NUM_AXES_SHAPED] = // Damping factor
|
||||
{ FTM_SHAPING_ZETA_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_ZETA_Y) };
|
||||
float vtol[NUM_AXES_SHAPED] = // Vibration Level
|
||||
{ FTM_SHAPING_V_TOL_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_V_TOL_Y) };
|
||||
ft_shaped_shaper_t shaper = // Shaper type
|
||||
{ SHAPED_ELEM(FTM_DEFAULT_SHAPER_X, FTM_DEFAULT_SHAPER_Y) };
|
||||
ft_shaped_float_t baseFreq = // Base frequency. [Hz]
|
||||
{ SHAPED_ELEM(FTM_SHAPING_DEFAULT_X_FREQ, FTM_SHAPING_DEFAULT_Y_FREQ) };
|
||||
ft_shaped_float_t zeta = // Damping factor
|
||||
{ SHAPED_ELEM(FTM_SHAPING_ZETA_X, FTM_SHAPING_ZETA_Y) };
|
||||
ft_shaped_float_t vtol = // Vibration Level
|
||||
{ SHAPED_ELEM(FTM_SHAPING_V_TOL_X, FTM_SHAPING_V_TOL_Y) };
|
||||
#endif
|
||||
|
||||
#if HAS_DYNAMIC_FREQ
|
||||
dynFreqMode_t dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; // Dynamic frequency mode configuration.
|
||||
float dynFreqK[NUM_AXES_SHAPED] = { 0.0f }; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g]
|
||||
ft_shaped_float_t dynFreqK = { 0.0f }; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g]
|
||||
#else
|
||||
static constexpr dynFreqMode_t dynFreqMode = dynFreqMode_DISABLED;
|
||||
#endif
|
||||
@ -77,22 +75,23 @@ class FTMotion {
|
||||
cfg.active = ENABLED(FTM_IS_DEFAULT_MOTION);
|
||||
|
||||
#if HAS_X_AXIS
|
||||
cfg.shaper[X_AXIS] = FTM_DEFAULT_SHAPER_X;
|
||||
cfg.baseFreq[X_AXIS] = FTM_SHAPING_DEFAULT_X_FREQ;
|
||||
cfg.zeta[X_AXIS] = FTM_SHAPING_ZETA_X;
|
||||
cfg.vtol[X_AXIS] = FTM_SHAPING_V_TOL_X;
|
||||
cfg.shaper.x = FTM_DEFAULT_SHAPER_X;
|
||||
cfg.baseFreq.x = FTM_SHAPING_DEFAULT_X_FREQ;
|
||||
cfg.zeta.x = FTM_SHAPING_ZETA_X;
|
||||
cfg.vtol.x = FTM_SHAPING_V_TOL_X;
|
||||
#endif
|
||||
|
||||
#if HAS_Y_AXIS
|
||||
cfg.shaper[Y_AXIS] = FTM_DEFAULT_SHAPER_Y;
|
||||
cfg.baseFreq[Y_AXIS] = FTM_SHAPING_DEFAULT_Y_FREQ;
|
||||
cfg.zeta[Y_AXIS] = FTM_SHAPING_ZETA_Y;
|
||||
cfg.vtol[Y_AXIS] = FTM_SHAPING_V_TOL_Y;
|
||||
cfg.shaper.y = FTM_DEFAULT_SHAPER_Y;
|
||||
cfg.baseFreq.y = FTM_SHAPING_DEFAULT_Y_FREQ;
|
||||
cfg.zeta.y = FTM_SHAPING_ZETA_Y;
|
||||
cfg.vtol.y = FTM_SHAPING_V_TOL_Y;
|
||||
#endif
|
||||
|
||||
#if HAS_DYNAMIC_FREQ
|
||||
cfg.dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE;
|
||||
cfg.dynFreqK[X_AXIS] = TERN_(HAS_Y_AXIS, cfg.dynFreqK[Y_AXIS]) = 0.0f;
|
||||
TERN_(HAS_X_AXIS, cfg.dynFreqK.x = 0.0f);
|
||||
TERN_(HAS_Y_AXIS, cfg.dynFreqK.y = 0.0f);
|
||||
#endif
|
||||
|
||||
#if HAS_EXTRUDERS
|
||||
@ -143,7 +142,8 @@ class FTMotion {
|
||||
static uint32_t N1, N2, N3;
|
||||
static uint32_t max_intervals;
|
||||
|
||||
static constexpr uint32_t PROP_BATCHES = CEIL(FTM_WINDOW_SIZE/FTM_BATCH_SIZE) - 1; // Number of batches needed to propagate the current trajectory to the stepper.
|
||||
// Number of batches needed to propagate the current trajectory to the stepper.
|
||||
static constexpr uint32_t PROP_BATCHES = CEIL((FTM_WINDOW_SIZE) / (FTM_BATCH_SIZE)) - 1;
|
||||
|
||||
// Make vector variables.
|
||||
static uint32_t makeVector_idx,
|
||||
@ -195,7 +195,7 @@ class FTMotion {
|
||||
static void makeVector();
|
||||
static void convertToSteps(const uint32_t idx);
|
||||
|
||||
FORCE_INLINE static int32_t num_samples_cmpnstr_settle() { return ( shaping.x.ena || shaping.y.ena ) ? FTM_ZMAX : 0; }
|
||||
FORCE_INLINE static int32_t num_samples_shaper_settle() { return ( shaping.x.ena || shaping.y.ena ) ? FTM_ZMAX : 0; }
|
||||
|
||||
|
||||
}; // class FTMotion
|
||||
|
||||
@ -23,7 +23,7 @@
|
||||
|
||||
#include "../core/types.h"
|
||||
|
||||
typedef enum FXDTICtrlShaper : uint8_t {
|
||||
enum ftMotionShaper_t : uint8_t {
|
||||
ftMotionShaper_NONE = 0, // No compensator
|
||||
ftMotionShaper_ZV = 1, // Zero Vibration
|
||||
ftMotionShaper_ZVD = 2, // Zero Vibration and Derivative
|
||||
@ -33,7 +33,7 @@ typedef enum FXDTICtrlShaper : uint8_t {
|
||||
ftMotionShaper_2HEI = 6, // 2-Hump Extra-Intensive
|
||||
ftMotionShaper_3HEI = 7, // 3-Hump Extra-Intensive
|
||||
ftMotionShaper_MZV = 8 // Modified Zero Vibration
|
||||
} ftMotionShaper_t;
|
||||
};
|
||||
|
||||
enum dynFreqMode_t : uint8_t {
|
||||
dynFreqMode_DISABLED = 0,
|
||||
@ -59,4 +59,21 @@ enum {
|
||||
FT_BIT_COUNT
|
||||
};
|
||||
|
||||
#define NUM_AXES_SHAPED TERN(HAS_Y_AXIS, 2, 1)
|
||||
#define SHAPED_ELEM(A, B) A OPTARG(HAS_Y_AXIS, B)
|
||||
|
||||
template<typename T>
|
||||
struct FTShapedAxes {
|
||||
union {
|
||||
struct { T SHAPED_ELEM(X, Y); };
|
||||
struct { T SHAPED_ELEM(x, y); };
|
||||
T val[NUM_AXES_SHAPED];
|
||||
};
|
||||
T& operator[](int i) { return val[i]; }
|
||||
};
|
||||
|
||||
typedef FTShapedAxes<float> ft_shaped_float_t;
|
||||
typedef FTShapedAxes<ftMotionShaper_t> ft_shaped_shaper_t;
|
||||
typedef FTShapedAxes<dynFreqMode_t> ft_shaped_dfm_t;
|
||||
|
||||
typedef bits_t(FT_BIT_COUNT) ft_command_t;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user