|
@@ -215,14 +215,18 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
|
215
|
215
|
NOLESS(initial_rate, MINIMAL_STEP_RATE);
|
216
|
216
|
NOLESS(final_rate, MINIMAL_STEP_RATE);
|
217
|
217
|
|
218
|
|
- int32_t accel = block->acceleration_steps_per_s2,
|
219
|
|
- accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
|
|
218
|
+ const int32_t accel = block->acceleration_steps_per_s2;
|
|
219
|
+
|
|
220
|
+ // Steps required for acceleration, deceleration to/from nominal rate
|
|
221
|
+ int32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
|
220
|
222
|
decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel)),
|
|
223
|
+ // Steps between acceleration and deceleration, if any
|
221
|
224
|
plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
|
222
|
225
|
|
223
|
|
- // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
|
224
|
|
- // have to use intersection_distance() to calculate when to abort accel and start braking
|
225
|
|
- // in order to reach the final_rate exactly at the end of this block.
|
|
226
|
+ // Does accelerate_steps + decelerate_steps exceed step_event_count?
|
|
227
|
+ // Then we can't possibly reach the nominal rate, there will be no cruising.
|
|
228
|
+ // Use intersection_distance() to calculate accel / braking time in order to
|
|
229
|
+ // reach the final_rate exactly at the end of this block.
|
226
|
230
|
if (plateau_steps < 0) {
|
227
|
231
|
accelerate_steps = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
|
228
|
232
|
NOLESS(accelerate_steps, 0); // Check limits due to numerical round-off
|
|
@@ -1052,22 +1056,23 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
|
1052
|
1056
|
}
|
1053
|
1057
|
float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
|
1054
|
1058
|
|
1055
|
|
- // Calculate moves/second for this move. No divide by zero due to previous checks.
|
1056
|
|
- float inverse_mm_s = fr_mm_s * inverse_millimeters;
|
|
1059
|
+ // Calculate inverse time for this move. No divide by zero due to previous checks.
|
|
1060
|
+ // Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
|
|
1061
|
+ float inverse_secs = fr_mm_s * inverse_millimeters;
|
1057
|
1062
|
|
1058
|
1063
|
const uint8_t moves_queued = movesplanned();
|
1059
|
1064
|
|
1060
|
1065
|
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
|
1061
|
1066
|
#if ENABLED(SLOWDOWN) || ENABLED(ULTRA_LCD) || defined(XY_FREQUENCY_LIMIT)
|
1062
|
1067
|
// Segment time im micro seconds
|
1063
|
|
- uint32_t segment_time_us = LROUND(1000000.0 / inverse_mm_s);
|
|
1068
|
+ uint32_t segment_time_us = LROUND(1000000.0 / inverse_secs);
|
1064
|
1069
|
#endif
|
1065
|
1070
|
#if ENABLED(SLOWDOWN)
|
1066
|
1071
|
if (WITHIN(moves_queued, 2, (BLOCK_BUFFER_SIZE) / 2 - 1)) {
|
1067
|
1072
|
if (segment_time_us < min_segment_time_us) {
|
1068
|
1073
|
// buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
|
1069
|
1074
|
const uint32_t nst = segment_time_us + LROUND(2 * (min_segment_time_us - segment_time_us) / moves_queued);
|
1070
|
|
- inverse_mm_s = 1000000.0 / nst;
|
|
1075
|
+ inverse_secs = 1000000.0 / nst;
|
1071
|
1076
|
#if defined(XY_FREQUENCY_LIMIT) || ENABLED(ULTRA_LCD)
|
1072
|
1077
|
segment_time_us = nst;
|
1073
|
1078
|
#endif
|
|
@@ -1081,8 +1086,8 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
|
1081
|
1086
|
CRITICAL_SECTION_END
|
1082
|
1087
|
#endif
|
1083
|
1088
|
|
1084
|
|
- block->nominal_speed = block->millimeters * inverse_mm_s; // (mm/sec) Always > 0
|
1085
|
|
- block->nominal_rate = CEIL(block->step_event_count * inverse_mm_s); // (step/sec) Always > 0
|
|
1089
|
+ block->nominal_speed = block->millimeters * inverse_secs; // (mm/sec) Always > 0
|
|
1090
|
+ block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
|
1086
|
1091
|
|
1087
|
1092
|
#if ENABLED(FILAMENT_WIDTH_SENSOR)
|
1088
|
1093
|
static float filwidth_e_count = 0, filwidth_delay_dist = 0;
|
|
@@ -1121,7 +1126,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
|
1121
|
1126
|
// Calculate and limit speed in mm/sec for each axis
|
1122
|
1127
|
float current_speed[NUM_AXIS], speed_factor = 1.0; // factor <1 decreases speed
|
1123
|
1128
|
LOOP_XYZE(i) {
|
1124
|
|
- const float cs = FABS((current_speed[i] = delta_mm[i] * inverse_mm_s));
|
|
1129
|
+ const float cs = FABS((current_speed[i] = delta_mm[i] * inverse_secs));
|
1125
|
1130
|
#if ENABLED(DISTINCT_E_FACTORS)
|
1126
|
1131
|
if (i == E_AXIS) i += extruder;
|
1127
|
1132
|
#endif
|
|
@@ -1374,7 +1379,6 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
|
1374
|
1379
|
previous_safe_speed = safe_speed;
|
1375
|
1380
|
|
1376
|
1381
|
#if ENABLED(LIN_ADVANCE)
|
1377
|
|
-
|
1378
|
1382
|
/**
|
1379
|
1383
|
*
|
1380
|
1384
|
* Use LIN_ADVANCE for blocks if all these are true:
|