|
@@ -150,33 +150,31 @@ void Planner::init() {
|
150
|
150
|
* by the provided factors.
|
151
|
151
|
*/
|
152
|
152
|
void Planner::calculate_trapezoid_for_block(block_t* block, float entry_factor, float exit_factor) {
|
153
|
|
- unsigned long initial_rate = ceil(block->nominal_rate * entry_factor),
|
154
|
|
- final_rate = ceil(block->nominal_rate * exit_factor); // (steps per second)
|
|
153
|
+ uint32_t initial_rate = ceil(block->nominal_rate * entry_factor),
|
|
154
|
+ final_rate = ceil(block->nominal_rate * exit_factor); // (steps per second)
|
155
|
155
|
|
156
|
156
|
// Limit minimal step rate (Otherwise the timer will overflow.)
|
157
|
157
|
NOLESS(initial_rate, 120);
|
158
|
158
|
NOLESS(final_rate, 120);
|
159
|
159
|
|
160
|
|
- long accel = block->acceleration_steps_per_s2;
|
161
|
|
- int32_t accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel));
|
162
|
|
- int32_t decelerate_steps = floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel));
|
163
|
|
-
|
164
|
|
- // Calculate the size of Plateau of Nominal Rate.
|
165
|
|
- int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
|
|
160
|
+ int32_t accel = block->acceleration_steps_per_s2,
|
|
161
|
+ accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
|
|
162
|
+ decelerate_steps = floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel)),
|
|
163
|
+ plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
|
166
|
164
|
|
167
|
165
|
// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
|
168
|
166
|
// have to use intersection_distance() to calculate when to abort accel and start braking
|
169
|
167
|
// in order to reach the final_rate exactly at the end of this block.
|
170
|
168
|
if (plateau_steps < 0) {
|
171
|
169
|
accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
|
172
|
|
- accelerate_steps = max(accelerate_steps, 0); // Check limits due to numerical round-off
|
|
170
|
+ NOLESS(accelerate_steps, 0); // Check limits due to numerical round-off
|
173
|
171
|
accelerate_steps = min((uint32_t)accelerate_steps, block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero)
|
174
|
172
|
plateau_steps = 0;
|
175
|
173
|
}
|
176
|
174
|
|
177
|
175
|
#if ENABLED(ADVANCE)
|
178
|
|
- volatile long initial_advance = block->advance * sq(entry_factor);
|
179
|
|
- volatile long final_advance = block->advance * sq(exit_factor);
|
|
176
|
+ volatile int32_t initial_advance = block->advance * sq(entry_factor),
|
|
177
|
+ final_advance = block->advance * sq(exit_factor);
|
180
|
178
|
#endif // ADVANCE
|
181
|
179
|
|
182
|
180
|
// block->accelerate_until = accelerate_steps;
|
|
@@ -266,7 +264,7 @@ void Planner::forward_pass_kernel(block_t* previous, block_t* current) {
|
266
|
264
|
// If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
|
267
|
265
|
if (!previous->nominal_length_flag) {
|
268
|
266
|
if (previous->entry_speed < current->entry_speed) {
|
269
|
|
- double entry_speed = min(current->entry_speed,
|
|
267
|
+ float entry_speed = min(current->entry_speed,
|
270
|
268
|
max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters));
|
271
|
269
|
// Check for junction speed change
|
272
|
270
|
if (current->entry_speed != entry_speed) {
|
|
@@ -982,15 +980,13 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
|
982
|
980
|
#endif
|
983
|
981
|
|
984
|
982
|
// Calculate and limit speed in mm/sec for each axis
|
985
|
|
- float current_speed[NUM_AXIS];
|
986
|
|
- float speed_factor = 1.0; //factor <=1 do decrease speed
|
|
983
|
+ float current_speed[NUM_AXIS], speed_factor = 1.0; // factor <1 decreases speed
|
987
|
984
|
LOOP_XYZE(i) {
|
988
|
|
- current_speed[i] = delta_mm[i] * inverse_mm_s;
|
989
|
|
- float cs = fabs(current_speed[i]), mf = max_feedrate_mm_s[i];
|
990
|
|
- if (cs > mf) speed_factor = min(speed_factor, mf / cs);
|
|
985
|
+ float cs = fabs(current_speed[i] = delta_mm[i] * inverse_mm_s);
|
|
986
|
+ if (cs > max_feedrate_mm_s[i]) NOMORE(speed_factor, max_feedrate_mm_s[i] / cs);
|
991
|
987
|
}
|
992
|
988
|
|
993
|
|
- // Max segement time in us.
|
|
989
|
+ // Max segment time in µs.
|
994
|
990
|
#ifdef XY_FREQUENCY_LIMIT
|
995
|
991
|
|
996
|
992
|
// Check and limit the xy direction change frequency
|
|
@@ -1024,7 +1020,7 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
|
1024
|
1020
|
min_xy_segment_time = min(max_x_segment_time, max_y_segment_time);
|
1025
|
1021
|
if (min_xy_segment_time < MAX_FREQ_TIME) {
|
1026
|
1022
|
float low_sf = speed_factor * min_xy_segment_time / (MAX_FREQ_TIME);
|
1027
|
|
- speed_factor = min(speed_factor, low_sf);
|
|
1023
|
+ NOMORE(speed_factor, low_sf);
|
1028
|
1024
|
}
|
1029
|
1025
|
#endif // XY_FREQUENCY_LIMIT
|
1030
|
1026
|
|
|
@@ -1091,8 +1087,7 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
|
1091
|
1087
|
if (cos_theta > -0.95) {
|
1092
|
1088
|
// Compute maximum junction velocity based on maximum acceleration and junction deviation
|
1093
|
1089
|
double sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
|
1094
|
|
- vmax_junction = min(vmax_junction,
|
1095
|
|
- sqrt(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)));
|
|
1090
|
+ NOMORE(vmax_junction, sqrt(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)));
|
1096
|
1091
|
}
|
1097
|
1092
|
}
|
1098
|
1093
|
}
|
|
@@ -1125,7 +1120,7 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
|
1125
|
1120
|
block->max_entry_speed = vmax_junction;
|
1126
|
1121
|
|
1127
|
1122
|
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
|
1128
|
|
- double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
|
|
1123
|
+ float v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
|
1129
|
1124
|
block->entry_speed = min(vmax_junction, v_allowable);
|
1130
|
1125
|
|
1131
|
1126
|
// Initialize planner efficiency flags
|