|
|
@ -204,14 +204,18 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
|
|
|
|
NOLESS(initial_rate, MINIMAL_STEP_RATE);
|
|
|
|
NOLESS(initial_rate, MINIMAL_STEP_RATE);
|
|
|
|
NOLESS(final_rate, MINIMAL_STEP_RATE);
|
|
|
|
NOLESS(final_rate, MINIMAL_STEP_RATE);
|
|
|
|
|
|
|
|
|
|
|
|
int32_t accel = block->acceleration_steps_per_s2,
|
|
|
|
const int32_t accel = block->acceleration_steps_per_s2;
|
|
|
|
accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
|
|
|
|
|
|
|
|
|
|
|
|
// Steps required for acceleration, deceleration to/from nominal rate
|
|
|
|
|
|
|
|
int32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
|
|
|
|
decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel)),
|
|
|
|
decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel)),
|
|
|
|
|
|
|
|
// Steps between acceleration and deceleration, if any
|
|
|
|
plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
|
|
|
|
plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
|
|
|
|
|
|
|
|
|
|
|
|
// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
|
|
|
|
// Does accelerate_steps + decelerate_steps exceed step_event_count?
|
|
|
|
// have to use intersection_distance() to calculate when to abort accel and start braking
|
|
|
|
// Then we can't possibly reach the nominal rate, there will be no cruising.
|
|
|
|
// in order to reach the final_rate exactly at the end of this block.
|
|
|
|
// Use intersection_distance() to calculate accel / braking time in order to
|
|
|
|
|
|
|
|
// reach the final_rate exactly at the end of this block.
|
|
|
|
if (plateau_steps < 0) {
|
|
|
|
if (plateau_steps < 0) {
|
|
|
|
accelerate_steps = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
|
|
|
|
accelerate_steps = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
|
|
|
|
NOLESS(accelerate_steps, 0); // Check limits due to numerical round-off
|
|
|
|
NOLESS(accelerate_steps, 0); // Check limits due to numerical round-off
|
|
|
@ -1045,22 +1049,23 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
|
|
|
|
}
|
|
|
|
}
|
|
|
|
float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
|
|
|
|
float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
|
|
|
|
|
|
|
|
|
|
|
|
// Calculate moves/second for this move. No divide by zero due to previous checks.
|
|
|
|
// Calculate inverse time for this move. No divide by zero due to previous checks.
|
|
|
|
float inverse_mm_s = fr_mm_s * inverse_millimeters;
|
|
|
|
// Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
|
|
|
|
|
|
|
|
float inverse_secs = fr_mm_s * inverse_millimeters;
|
|
|
|
|
|
|
|
|
|
|
|
const uint8_t moves_queued = movesplanned();
|
|
|
|
const uint8_t moves_queued = movesplanned();
|
|
|
|
|
|
|
|
|
|
|
|
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
|
|
|
|
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
|
|
|
|
#if ENABLED(SLOWDOWN) || ENABLED(ULTRA_LCD) || defined(XY_FREQUENCY_LIMIT)
|
|
|
|
#if ENABLED(SLOWDOWN) || ENABLED(ULTRA_LCD) || defined(XY_FREQUENCY_LIMIT)
|
|
|
|
// Segment time im micro seconds
|
|
|
|
// Segment time im micro seconds
|
|
|
|
uint32_t segment_time_us = LROUND(1000000.0 / inverse_mm_s);
|
|
|
|
uint32_t segment_time_us = LROUND(1000000.0 / inverse_secs);
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
#if ENABLED(SLOWDOWN)
|
|
|
|
#if ENABLED(SLOWDOWN)
|
|
|
|
if (WITHIN(moves_queued, 2, (BLOCK_BUFFER_SIZE) / 2 - 1)) {
|
|
|
|
if (WITHIN(moves_queued, 2, (BLOCK_BUFFER_SIZE) / 2 - 1)) {
|
|
|
|
if (segment_time_us < min_segment_time_us) {
|
|
|
|
if (segment_time_us < min_segment_time_us) {
|
|
|
|
// buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
|
|
|
|
// buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
|
|
|
|
const uint32_t nst = segment_time_us + LROUND(2 * (min_segment_time_us - segment_time_us) / moves_queued);
|
|
|
|
const uint32_t nst = segment_time_us + LROUND(2 * (min_segment_time_us - segment_time_us) / moves_queued);
|
|
|
|
inverse_mm_s = 1000000.0 / nst;
|
|
|
|
inverse_secs = 1000000.0 / nst;
|
|
|
|
#if defined(XY_FREQUENCY_LIMIT) || ENABLED(ULTRA_LCD)
|
|
|
|
#if defined(XY_FREQUENCY_LIMIT) || ENABLED(ULTRA_LCD)
|
|
|
|
segment_time_us = nst;
|
|
|
|
segment_time_us = nst;
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
@ -1074,8 +1079,8 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
|
|
|
|
CRITICAL_SECTION_END
|
|
|
|
CRITICAL_SECTION_END
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
block->nominal_speed = block->millimeters * inverse_mm_s; // (mm/sec) Always > 0
|
|
|
|
block->nominal_speed = block->millimeters * inverse_secs; // (mm/sec) Always > 0
|
|
|
|
block->nominal_rate = CEIL(block->step_event_count * inverse_mm_s); // (step/sec) Always > 0
|
|
|
|
block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(FILAMENT_WIDTH_SENSOR)
|
|
|
|
#if ENABLED(FILAMENT_WIDTH_SENSOR)
|
|
|
|
static float filwidth_e_count = 0, filwidth_delay_dist = 0;
|
|
|
|
static float filwidth_e_count = 0, filwidth_delay_dist = 0;
|
|
|
@ -1114,7 +1119,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
|
|
|
|
// Calculate and limit speed in mm/sec for each axis
|
|
|
|
// Calculate and limit speed in mm/sec for each axis
|
|
|
|
float current_speed[NUM_AXIS], speed_factor = 1.0; // factor <1 decreases speed
|
|
|
|
float current_speed[NUM_AXIS], speed_factor = 1.0; // factor <1 decreases speed
|
|
|
|
LOOP_XYZE(i) {
|
|
|
|
LOOP_XYZE(i) {
|
|
|
|
const float cs = FABS((current_speed[i] = delta_mm[i] * inverse_mm_s));
|
|
|
|
const float cs = FABS((current_speed[i] = delta_mm[i] * inverse_secs));
|
|
|
|
#if ENABLED(DISTINCT_E_FACTORS)
|
|
|
|
#if ENABLED(DISTINCT_E_FACTORS)
|
|
|
|
if (i == E_AXIS) i += extruder;
|
|
|
|
if (i == E_AXIS) i += extruder;
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|