bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

🚀 ZV Input Shaping (#24797)

Browse Source
This commit is contained in:
tombrazier
2022-10-21 22:34:22 +01:00
committed by Scott Lahteine
parent 5765449867
commit c1f0f26bff
15 changed files with 657 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

135
Marlin/src/module/stepper.h
View File

@ -312,6 +312,117 @@ constexpr ena_mask_t enable_overlap[] = {
//static_assert(!any_enable_overlap(), "There is some overlap.");
#if ENABLED(INPUT_SHAPING)
typedef IF<ENABLED(__AVR__), uint16_t, uint32_t>::type shaping_time_t;
// These constexpr are used to calculate the shaping queue buffer sizes
constexpr xyze_float_t max_feedrate = DEFAULT_MAX_FEEDRATE;
constexpr xyze_float_t steps_per_unit = DEFAULT_AXIS_STEPS_PER_UNIT;
constexpr float max_steprate = _MAX(LOGICAL_AXIS_LIST(
max_feedrate.e * steps_per_unit.e,
max_feedrate.x * steps_per_unit.x,
max_feedrate.y * steps_per_unit.y,
max_feedrate.z * steps_per_unit.z,
max_feedrate.i * steps_per_unit.i,
max_feedrate.j * steps_per_unit.j,
max_feedrate.k * steps_per_unit.k,
max_feedrate.u * steps_per_unit.u,
max_feedrate.v * steps_per_unit.v,
max_feedrate.w * steps_per_unit.w
));
constexpr uint16_t shaping_dividends = max_steprate / _MIN(0x7FFFFFFFL OPTARG(HAS_SHAPING_X, SHAPING_FREQ_X) OPTARG(HAS_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3;
constexpr uint16_t shaping_segments = max_steprate / (MIN_STEPS_PER_SEGMENT) / _MIN(0x7FFFFFFFL OPTARG(HAS_SHAPING_X, SHAPING_FREQ_X) OPTARG(HAS_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3;
class DelayTimeManager {
private:
static shaping_time_t now;
#ifdef HAS_SHAPING_X
static shaping_time_t delay_x;
#endif
#ifdef HAS_SHAPING_Y
static shaping_time_t delay_y;
#endif
public:
static void decrement_delays(const shaping_time_t interval) { now += interval; }
static void set_delay(const AxisEnum axis, const shaping_time_t delay) {
TERN_(HAS_SHAPING_X, if (axis == X_AXIS) delay_x = delay);
TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) delay_y = delay);
}
};
template<int SIZE>
class DelayQueue : public DelayTimeManager {
protected:
shaping_time_t times[SIZE];
uint16_t tail = 0 OPTARG(HAS_SHAPING_X, head_x = 0) OPTARG(HAS_SHAPING_Y, head_y = 0);
public:
void enqueue() {
times[tail] = now;
if (++tail == SIZE) tail = 0;
}
#ifdef HAS_SHAPING_X
shaping_time_t peek_x() {
if (head_x != tail) return times[head_x] + delay_x - now;
else return shaping_time_t(-1);
}
void dequeue_x() { if (++head_x == SIZE) head_x = 0; }
bool empty_x() { return head_x == tail; }
uint16_t free_count_x() { return head_x > tail ? head_x - tail - 1 : head_x + SIZE - tail - 1; }
#endif
#ifdef HAS_SHAPING_Y
shaping_time_t peek_y() {
if (head_y != tail) return times[head_y] + delay_y - now;
else return shaping_time_t(-1);
}
void dequeue_y() { if (++head_y == SIZE) head_y = 0; }
bool empty_y() { return head_y == tail; }
uint16_t free_count_y() { return head_y > tail ? head_y - tail - 1 : head_y + SIZE - tail - 1; }
#endif
void purge() { auto temp = TERN_(HAS_SHAPING_X, head_x) = TERN_(HAS_SHAPING_Y, head_y) = tail; UNUSED(temp);}
};
class ParamDelayQueue : public DelayQueue<shaping_segments> {
private:
#ifdef HAS_SHAPING_X
int32_t params_x[shaping_segments];
#endif
#ifdef HAS_SHAPING_Y
int32_t params_y[shaping_segments];
#endif
public:
void enqueue(const int32_t param_x, const int32_t param_y) {
TERN(HAS_SHAPING_X, params_x[DelayQueue<shaping_segments>::tail] = param_x, UNUSED(param_x));
TERN(HAS_SHAPING_Y, params_y[DelayQueue<shaping_segments>::tail] = param_y, UNUSED(param_y));
DelayQueue<shaping_segments>::enqueue();
}
#ifdef HAS_SHAPING_X
const int32_t dequeue_x() {
const int32_t result = params_x[DelayQueue<shaping_segments>::head_x];
DelayQueue<shaping_segments>::dequeue_x();
return result;
}
#endif
#ifdef HAS_SHAPING_Y
const int32_t dequeue_y() {
const int32_t result = params_y[DelayQueue<shaping_segments>::head_y];
DelayQueue<shaping_segments>::dequeue_y();
return result;
}
#endif
};
struct ShapeParams {
float frequency;
float zeta;
uint8_t factor;
int32_t dividend;
};
#endif // INPUT_SHAPING
//
// Stepper class definition
//
@ -391,7 +502,7 @@ class Stepper {
// Delta error variables for the Bresenham line tracer
static xyze_long_t delta_error;
static xyze_ulong_t advance_dividend;
static xyze_long_t advance_dividend;
static uint32_t advance_divisor,
step_events_completed, // The number of step events executed in the current block
accelerate_until, // The point from where we need to stop acceleration
@ -416,6 +527,17 @@ class Stepper {
static bool bezier_2nd_half; // If Bézier curve has been initialized or not
#endif
#if ENABLED(INPUT_SHAPING)
static ParamDelayQueue shaping_dividend_queue;
static DelayQueue<shaping_dividends> shaping_queue;
#if HAS_SHAPING_X
static ShapeParams shaping_x;
#endif
#if HAS_SHAPING_Y
static ShapeParams shaping_y;
#endif
#endif
#if ENABLED(LIN_ADVANCE)
static constexpr uint32_t LA_ADV_NEVER = 0xFFFFFFFF;
static uint32_t nextAdvanceISR,
@ -475,6 +597,10 @@ class Stepper {
// The stepper block processing ISR phase
static uint32_t block_phase_isr();
#if ENABLED(INPUT_SHAPING)
static void shaping_isr();
#endif
#if ENABLED(LIN_ADVANCE)
// The Linear advance ISR phase
static void advance_isr();
@ -628,6 +754,13 @@ class Stepper {
set_directions();
}
#if ENABLED(INPUT_SHAPING)
static void set_shaping_damping_ratio(const AxisEnum axis, const float zeta);
static float get_shaping_damping_ratio(const AxisEnum axis);
static void set_shaping_frequency(const AxisEnum axis, const float freq);
static float get_shaping_frequency(const AxisEnum axis);
#endif
private:
// Set the current position in steps