sravan/Marlin-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

✨ INPUT_SHAPING_Z (#27073)

Browse Source
This commit is contained in:
Jonathan Brazier
2024-05-20 06:03:03 +01:00
committed by GitHub
parent 6c018eb770
commit 6b6865d068
11 changed files with 250 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files

111
Marlin/src/module/stepper.cpp
View File

@@ -281,20 +281,16 @@ uint32_t Stepper::advance_divisor = 0,
shaping_echo_axis_t ShapingQueue::echo_axes[shaping_echoes];
uint16_t ShapingQueue::tail = 0;
#if ENABLED(INPUT_SHAPING_X)
shaping_time_t ShapingQueue::delay_x;
shaping_time_t ShapingQueue::peek_x_val = shaping_time_t(-1);
uint16_t ShapingQueue::head_x = 0;
uint16_t ShapingQueue::_free_count_x = shaping_echoes - 1;
ShapeParams Stepper::shaping_x;
#endif
#if ENABLED(INPUT_SHAPING_Y)
shaping_time_t ShapingQueue::delay_y;
shaping_time_t ShapingQueue::peek_y_val = shaping_time_t(-1);
uint16_t ShapingQueue::head_y = 0;
uint16_t ShapingQueue::_free_count_y = shaping_echoes - 1;
ShapeParams Stepper::shaping_y;
#endif
#define SHAPING_VAR_DEFS(AXIS) \
shaping_time_t ShapingQueue::delay_##AXIS; \
shaping_time_t ShapingQueue::_peek_##AXIS = shaping_time_t(-1); \
uint16_t ShapingQueue::head_##AXIS = 0; \
uint16_t ShapingQueue::_free_count_##AXIS = shaping_echoes - 1; \
ShapeParams Stepper::shaping_##AXIS;
TERN_(INPUT_SHAPING_X, SHAPING_VAR_DEFS(x))
TERN_(INPUT_SHAPING_Y, SHAPING_VAR_DEFS(y))
TERN_(INPUT_SHAPING_Z, SHAPING_VAR_DEFS(z))
#endif
#if ENABLED(BABYSTEPPING)
@@ -1610,6 +1606,7 @@ void Stepper::isr() {
interval = _MIN(nextMainISR, uint32_t(HAL_TIMER_TYPE_MAX)); // Time until the next Pulse / Block phase
TERN_(INPUT_SHAPING_X, NOMORE(interval, ShapingQueue::peek_x())); // Time until next input shaping echo for X
TERN_(INPUT_SHAPING_Y, NOMORE(interval, ShapingQueue::peek_y())); // Time until next input shaping echo for Y
TERN_(INPUT_SHAPING_Z, NOMORE(interval, ShapingQueue::peek_z())); // Time until next input shaping echo for Z
TERN_(LIN_ADVANCE, NOMORE(interval, nextAdvanceISR)); // Come back early for Linear Advance?
TERN_(BABYSTEPPING, NOMORE(interval, nextBabystepISR)); // Come back early for Babystepping?
@@ -1754,6 +1751,10 @@ void Stepper::pulse_phase_isr() {
shaping_y.delta_error = 0;
shaping_y.last_block_end_pos = count_position.y;
#endif
#if ENABLED(INPUT_SHAPING_Z)
shaping_z.delta_error = 0;
shaping_z.last_block_end_pos = count_position.z;
#endif
#endif
}
}
@@ -1813,6 +1814,12 @@ void Stepper::pulse_phase_isr() {
#else
#define HYSTERESIS_Y 0
#endif
#if AXIS_DRIVER_TYPE_Z(TMC2208) || AXIS_DRIVER_TYPE_Z(TMC2208_STANDALONE) || \
AXIS_DRIVER_TYPE_Z(TMC5160) || AXIS_DRIVER_TYPE_Z(TMC5160_STANDALONE)
#define HYSTERESIS_Z 64
#else
#define HYSTERESIS_Z 0
#endif
#define _HYSTERESIS(AXIS) HYSTERESIS_##AXIS
#define HYSTERESIS(AXIS) _HYSTERESIS(AXIS)
@@ -2005,9 +2012,10 @@ void Stepper::pulse_phase_isr() {
#if HAS_ZV_SHAPING
// record an echo if a step is needed in the primary bresenham
const bool x_step = TERN0(INPUT_SHAPING_X, step_needed.x && shaping_x.enabled),
y_step = TERN0(INPUT_SHAPING_Y, step_needed.y && shaping_y.enabled);
if (x_step || y_step)
ShapingQueue::enqueue(x_step, TERN0(INPUT_SHAPING_X, shaping_x.forward), y_step, TERN0(INPUT_SHAPING_Y, shaping_y.forward));
y_step = TERN0(INPUT_SHAPING_Y, step_needed.y && shaping_y.enabled),
z_step = TERN0(INPUT_SHAPING_Z, step_needed.z && shaping_z.enabled);
if (x_step || y_step || z_step)
ShapingQueue::enqueue(x_step, TERN0(INPUT_SHAPING_X, shaping_x.forward), y_step, TERN0(INPUT_SHAPING_Y, shaping_y.forward), z_step, TERN0(INPUT_SHAPING_Z, shaping_z.forward));
// do the first part of the secondary bresenham
#if ENABLED(INPUT_SHAPING_X)
@@ -2018,6 +2026,10 @@ void Stepper::pulse_phase_isr() {
if (y_step)
PULSE_PREP_SHAPING(Y, shaping_y.delta_error, shaping_y.forward ? shaping_y.factor1 : -shaping_y.factor1);
#endif
#if ENABLED(INPUT_SHAPING_Z)
if (z_step)
PULSE_PREP_SHAPING(Z, shaping_z.delta_error, shaping_z.forward ? shaping_z.factor1 : -shaping_z.factor1);
#endif
#endif
}
@@ -2124,6 +2136,7 @@ void Stepper::pulse_phase_isr() {
// Clear the echoes that are ready to process. If the buffers are too full and risk overflow, also apply echoes early.
TERN_(INPUT_SHAPING_X, step_needed.x = !ShapingQueue::peek_x() || ShapingQueue::free_count_x() < steps_per_isr);
TERN_(INPUT_SHAPING_Y, step_needed.y = !ShapingQueue::peek_y() || ShapingQueue::free_count_y() < steps_per_isr);
TERN_(INPUT_SHAPING_Z, step_needed.z = !ShapingQueue::peek_z() || ShapingQueue::free_count_z() < steps_per_isr);
if (bool(step_needed)) while (true) {
#if ENABLED(INPUT_SHAPING_X)
@@ -2142,6 +2155,14 @@ void Stepper::pulse_phase_isr() {
}
#endif
#if ENABLED(INPUT_SHAPING_Z)
if (step_needed.z) {
const bool forward = ShapingQueue::dequeue_z();
PULSE_PREP_SHAPING(Z, shaping_z.delta_error, (forward ? shaping_z.factor2 : -shaping_z.factor2));
PULSE_START(Z);
}
#endif
TERN_(I2S_STEPPER_STREAM, i2s_push_sample());
USING_TIMED_PULSE();
@@ -2156,10 +2177,14 @@ void Stepper::pulse_phase_isr() {
#if ENABLED(INPUT_SHAPING_Y)
PULSE_STOP(Y);
#endif
#if ENABLED(INPUT_SHAPING_Z)
PULSE_STOP(Z);
#endif
}
TERN_(INPUT_SHAPING_X, step_needed.x = !ShapingQueue::peek_x() || ShapingQueue::free_count_x() < steps_per_isr);
TERN_(INPUT_SHAPING_Y, step_needed.y = !ShapingQueue::peek_y() || ShapingQueue::free_count_y() < steps_per_isr);
TERN_(INPUT_SHAPING_Z, step_needed.z = !ShapingQueue::peek_z() || ShapingQueue::free_count_z() < steps_per_isr);
if (!bool(step_needed)) break;
@@ -2708,7 +2733,7 @@ hal_timer_t Stepper::block_phase_isr() {
}
#endif
// Y follows the same logic as X (but the comments aren't repeated)
// Y and Z follow the same logic as X (but the comments aren't repeated)
#if ENABLED(INPUT_SHAPING_Y)
if (shaping_y.enabled) {
const int64_t steps = current_block->direction_bits.y ? int64_t(current_block->steps.y) : -int64_t(current_block->steps.y);
@@ -2718,6 +2743,15 @@ hal_timer_t Stepper::block_phase_isr() {
}
#endif
#if ENABLED(INPUT_SHAPING_Z)
if (shaping_z.enabled) {
const int64_t steps = current_block->direction_bits.z ? int64_t(current_block->steps.z) : -int64_t(current_block->steps.z);
shaping_z.last_block_end_pos += steps;
shaping_z.forward = current_block->direction_bits.z;
if (!ShapingQueue::empty_z()) current_block->direction_bits.z = last_direction_bits.z;
}
#endif
// No step events completed so far
step_events_completed = 0;
@@ -3220,12 +3254,14 @@ void Stepper::init() {
hal.isr_off();
TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) { shaping_x.factor2 = factor2; shaping_x.factor1 = 128 - factor2; shaping_x.zeta = zeta; })
TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) { shaping_y.factor2 = factor2; shaping_y.factor1 = 128 - factor2; shaping_y.zeta = zeta; })
TERN_(INPUT_SHAPING_Z, if (axis == Z_AXIS) { shaping_z.factor2 = factor2; shaping_z.factor1 = 128 - factor2; shaping_z.zeta = zeta; })
if (was_on) hal.isr_on();
}
float Stepper::get_shaping_damping_ratio(const AxisEnum axis) {
TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) return shaping_x.zeta);
TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.zeta);
TERN_(INPUT_SHAPING_Z, if (axis == Z_AXIS) return shaping_z.zeta);
return -1;
}
@@ -3237,24 +3273,18 @@ void Stepper::init() {
hal.isr_off();
const shaping_time_t delay = freq ? float(uint32_t(STEPPER_TIMER_RATE) / 2) / freq : shaping_time_t(-1);
#if ENABLED(INPUT_SHAPING_X)
if (axis == X_AXIS) {
ShapingQueue::set_delay(X_AXIS, delay);
shaping_x.frequency = freq;
shaping_x.enabled = !!freq;
shaping_x.delta_error = 0;
shaping_x.last_block_end_pos = count_position.x;
#define SHAPING_SET_FREQ_FOR_AXIS(AXISN, AXISL) \
if (axis == AXISN) { \
ShapingQueue::set_delay(AXISN, delay); \
shaping_##AXISL.frequency = freq; \
shaping_##AXISL.enabled = !!freq; \
shaping_##AXISL.delta_error = 0; \
shaping_##AXISL.last_block_end_pos = count_position.AXISL; \
}
#endif
#if ENABLED(INPUT_SHAPING_Y)
if (axis == Y_AXIS) {
ShapingQueue::set_delay(Y_AXIS, delay);
shaping_y.frequency = freq;
shaping_y.enabled = !!freq;
shaping_y.delta_error = 0;
shaping_y.last_block_end_pos = count_position.y;
}
#endif
TERN_(INPUT_SHAPING_X, SHAPING_SET_FREQ_FOR_AXIS(X_AXIS, x))
TERN_(INPUT_SHAPING_Y, SHAPING_SET_FREQ_FOR_AXIS(Y_AXIS, y))
TERN_(INPUT_SHAPING_Z, SHAPING_SET_FREQ_FOR_AXIS(Z_AXIS, z))
if (was_on) hal.isr_on();
}
@@ -3262,6 +3292,7 @@ void Stepper::init() {
float Stepper::get_shaping_frequency(const AxisEnum axis) {
TERN_(INPUT_SHAPING_X, if (axis == X_AXIS) return shaping_x.frequency);
TERN_(INPUT_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.frequency);
TERN_(INPUT_SHAPING_Z, if (axis == Z_AXIS) return shaping_z.frequency);
return -1;
}
@@ -3283,6 +3314,9 @@ void Stepper::_set_position(const abce_long_t &spos) {
#if ENABLED(INPUT_SHAPING_Y)
const int32_t y_shaping_delta = count_position.y - shaping_y.last_block_end_pos;
#endif
#if ENABLED(INPUT_SHAPING_Z)
const int32_t z_shaping_delta = count_position.z - shaping_z.last_block_end_pos;
#endif
#if ANY(IS_CORE, MARKFORGED_XY, MARKFORGED_YX)
// Core equations follow the form of the dA and dB equations at https://www.corexy.com/theory.html
@@ -3323,6 +3357,12 @@ void Stepper::_set_position(const abce_long_t &spos) {
shaping_y.last_block_end_pos = spos.y;
}
#endif
#if ENABLED(INPUT_SHAPING_Z)
if (shaping_z.enabled) {
count_position.z += z_shaping_delta;
shaping_z.last_block_end_pos = spos.z;
}
#endif
}
/**
@@ -3364,6 +3404,7 @@ void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) {
count_position[a] = v;
TERN_(INPUT_SHAPING_X, if (a == X_AXIS) shaping_x.last_block_end_pos = v);
TERN_(INPUT_SHAPING_Y, if (a == Y_AXIS) shaping_y.last_block_end_pos = v);
TERN_(INPUT_SHAPING_Z, if (a == Z_AXIS) shaping_z.last_block_end_pos = v);
#ifdef __AVR__
// Reenable Stepper ISR