|
@@ -85,8 +85,8 @@ float Planner::max_feedrate_mm_s[NUM_AXIS], // Max speeds in mm per second
|
85
|
85
|
Planner::axis_steps_per_mm[NUM_AXIS],
|
86
|
86
|
Planner::steps_to_mm[NUM_AXIS];
|
87
|
87
|
|
88
|
|
-unsigned long Planner::max_acceleration_steps_per_s2[NUM_AXIS],
|
89
|
|
- Planner::max_acceleration_mm_per_s2[NUM_AXIS]; // Use M201 to override by software
|
|
88
|
+uint32_t Planner::max_acceleration_steps_per_s2[NUM_AXIS],
|
|
89
|
+ Planner::max_acceleration_mm_per_s2[NUM_AXIS]; // Use M201 to override by software
|
90
|
90
|
|
91
|
91
|
millis_t Planner::min_segment_time;
|
92
|
92
|
float Planner::min_feedrate_mm_s,
|
|
@@ -236,6 +236,7 @@ void Planner::reverse_pass() {
|
236
|
236
|
|
237
|
237
|
uint8_t b = BLOCK_MOD(block_buffer_head - 3);
|
238
|
238
|
while (b != tail) {
|
|
239
|
+ if (block[0] && (block[0]->flag & BLOCK_FLAG_START_FROM_FULL_HALT)) break;
|
239
|
240
|
b = prev_block_index(b);
|
240
|
241
|
block[2] = block[1];
|
241
|
242
|
block[1] = block[0];
|
|
@@ -696,6 +697,9 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
|
696
|
697
|
// Bail if this is a zero-length block
|
697
|
698
|
if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return;
|
698
|
699
|
|
|
700
|
+ // Clear the block flags
|
|
701
|
+ block->flag = 0;
|
|
702
|
+
|
699
|
703
|
// For a mixing extruder, get a magnified step_event_count for each
|
700
|
704
|
#if ENABLED(MIXING_EXTRUDER)
|
701
|
705
|
for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
|
|
@@ -1011,90 +1015,170 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
|
1011
|
1015
|
|
1012
|
1016
|
// Compute and limit the acceleration rate for the trapezoid generator.
|
1013
|
1017
|
float steps_per_mm = block->step_event_count / block->millimeters;
|
|
1018
|
+ uint32_t accel;
|
1014
|
1019
|
if (!block->steps[X_AXIS] && !block->steps[Y_AXIS] && !block->steps[Z_AXIS]) {
|
1015
|
|
- block->acceleration_steps_per_s2 = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
|
|
1020
|
+ // convert to: acceleration steps/sec^2
|
|
1021
|
+ accel = ceil(retract_acceleration * steps_per_mm);
|
1016
|
1022
|
}
|
1017
|
1023
|
else {
|
|
1024
|
+ #define LIMIT_ACCEL(AXIS) do{ \
|
|
1025
|
+ const uint32_t comp = max_acceleration_steps_per_s2[AXIS] * block->step_event_count; \
|
|
1026
|
+ if (accel * block->steps[AXIS] > comp) accel = comp / block->steps[AXIS]; \
|
|
1027
|
+ }while(0)
|
|
1028
|
+
|
|
1029
|
+ // Start with print or travel acceleration
|
|
1030
|
+ accel = ceil((block->steps[E_AXIS] ? acceleration : travel_acceleration) * steps_per_mm);
|
|
1031
|
+
|
1018
|
1032
|
// Limit acceleration per axis
|
1019
|
|
- block->acceleration_steps_per_s2 = ceil((block->steps[E_AXIS] ? acceleration : travel_acceleration) * steps_per_mm);
|
1020
|
|
- if (max_acceleration_steps_per_s2[X_AXIS] < (block->acceleration_steps_per_s2 * block->steps[X_AXIS]) / block->step_event_count)
|
1021
|
|
- block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[X_AXIS] * block->step_event_count) / block->steps[X_AXIS];
|
1022
|
|
- if (max_acceleration_steps_per_s2[Y_AXIS] < (block->acceleration_steps_per_s2 * block->steps[Y_AXIS]) / block->step_event_count)
|
1023
|
|
- block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[Y_AXIS] * block->step_event_count) / block->steps[Y_AXIS];
|
1024
|
|
- if (max_acceleration_steps_per_s2[Z_AXIS] < (block->acceleration_steps_per_s2 * block->steps[Z_AXIS]) / block->step_event_count)
|
1025
|
|
- block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[Z_AXIS] * block->step_event_count) / block->steps[Z_AXIS];
|
1026
|
|
- if (max_acceleration_steps_per_s2[E_AXIS] < (block->acceleration_steps_per_s2 * block->steps[E_AXIS]) / block->step_event_count)
|
1027
|
|
- block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[E_AXIS] * block->step_event_count) / block->steps[E_AXIS];
|
|
1033
|
+ LIMIT_ACCEL(X_AXIS);
|
|
1034
|
+ LIMIT_ACCEL(Y_AXIS);
|
|
1035
|
+ LIMIT_ACCEL(Z_AXIS);
|
|
1036
|
+ LIMIT_ACCEL(E_AXIS);
|
1028
|
1037
|
}
|
1029
|
|
- block->acceleration = block->acceleration_steps_per_s2 / steps_per_mm;
|
1030
|
|
- block->acceleration_rate = (long)(block->acceleration_steps_per_s2 * 16777216.0 / ((F_CPU) * 0.125));
|
|
1038
|
+ block->acceleration_steps_per_s2 = accel;
|
|
1039
|
+ block->acceleration = accel / steps_per_mm;
|
|
1040
|
+ block->acceleration_rate = (long)(accel * 16777216.0 / ((F_CPU) * 0.125)); // * 8.388608
|
|
1041
|
+
|
|
1042
|
+ // Initial limit on the segment entry velocity
|
|
1043
|
+ float vmax_junction;
|
1031
|
1044
|
|
1032
|
1045
|
#if 0 // Use old jerk for now
|
1033
|
1046
|
|
1034
|
1047
|
float junction_deviation = 0.1;
|
1035
|
1048
|
|
1036
|
1049
|
// Compute path unit vector
|
1037
|
|
- double unit_vec[XYZ];
|
1038
|
|
-
|
1039
|
|
- unit_vec[X_AXIS] = delta_mm[X_AXIS] * inverse_millimeters;
|
1040
|
|
- unit_vec[Y_AXIS] = delta_mm[Y_AXIS] * inverse_millimeters;
|
1041
|
|
- unit_vec[Z_AXIS] = delta_mm[Z_AXIS] * inverse_millimeters;
|
1042
|
|
-
|
1043
|
|
- // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
|
1044
|
|
- // Let a circle be tangent to both previous and current path line segments, where the junction
|
1045
|
|
- // deviation is defined as the distance from the junction to the closest edge of the circle,
|
1046
|
|
- // collinear with the circle center. The circular segment joining the two paths represents the
|
1047
|
|
- // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
|
1048
|
|
- // radius of the circle, defined indirectly by junction deviation. This may be also viewed as
|
1049
|
|
- // path width or max_jerk in the previous grbl version. This approach does not actually deviate
|
1050
|
|
- // from path, but used as a robust way to compute cornering speeds, as it takes into account the
|
1051
|
|
- // nonlinearities of both the junction angle and junction velocity.
|
1052
|
|
- double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
|
|
1050
|
+ double unit_vec[XYZ] = {
|
|
1051
|
+ delta_mm[X_AXIS] * inverse_millimeters,
|
|
1052
|
+ delta_mm[Y_AXIS] * inverse_millimeters,
|
|
1053
|
+ delta_mm[Z_AXIS] * inverse_millimeters
|
|
1054
|
+ };
|
|
1055
|
+
|
|
1056
|
+ /*
|
|
1057
|
+ Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
|
|
1058
|
+
|
|
1059
|
+ Let a circle be tangent to both previous and current path line segments, where the junction
|
|
1060
|
+ deviation is defined as the distance from the junction to the closest edge of the circle,
|
|
1061
|
+ collinear with the circle center.
|
|
1062
|
+
|
|
1063
|
+ The circular segment joining the two paths represents the path of centripetal acceleration.
|
|
1064
|
+ Solve for max velocity based on max acceleration about the radius of the circle, defined
|
|
1065
|
+ indirectly by junction deviation.
|
|
1066
|
+
|
|
1067
|
+ This may be also viewed as path width or max_jerk in the previous grbl version. This approach
|
|
1068
|
+ does not actually deviate from path, but used as a robust way to compute cornering speeds, as
|
|
1069
|
+ it takes into account the nonlinearities of both the junction angle and junction velocity.
|
|
1070
|
+ */
|
|
1071
|
+
|
|
1072
|
+ vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
|
1053
|
1073
|
|
1054
|
1074
|
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
|
1055
|
|
- if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
|
|
1075
|
+ if (block_buffer_head != block_buffer_tail && previous_nominal_speed > 0.0) {
|
1056
|
1076
|
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
|
1057
|
1077
|
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
|
1058
|
|
- double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
|
1059
|
|
- - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
|
1060
|
|
- - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
|
|
1078
|
+ float cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
|
|
1079
|
+ - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
|
|
1080
|
+ - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
|
1061
|
1081
|
// Skip and use default max junction speed for 0 degree acute junction.
|
1062
|
1082
|
if (cos_theta < 0.95) {
|
1063
|
1083
|
vmax_junction = min(previous_nominal_speed, block->nominal_speed);
|
1064
|
1084
|
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
|
1065
|
1085
|
if (cos_theta > -0.95) {
|
1066
|
1086
|
// Compute maximum junction velocity based on maximum acceleration and junction deviation
|
1067
|
|
- double sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
|
|
1087
|
+ float sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
|
1068
|
1088
|
NOMORE(vmax_junction, sqrt(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)));
|
1069
|
1089
|
}
|
1070
|
1090
|
}
|
1071
|
1091
|
}
|
1072
|
1092
|
#endif
|
1073
|
1093
|
|
1074
|
|
- // Start with a safe speed
|
1075
|
|
- float vmax_junction = max_jerk[X_AXIS] * 0.5, vmax_junction_factor = 1.0;
|
1076
|
|
- if (max_jerk[Y_AXIS] * 0.5 < fabs(current_speed[Y_AXIS])) NOMORE(vmax_junction, max_jerk[Y_AXIS] * 0.5);
|
1077
|
|
- if (max_jerk[Z_AXIS] * 0.5 < fabs(current_speed[Z_AXIS])) NOMORE(vmax_junction, max_jerk[Z_AXIS] * 0.5);
|
1078
|
|
- if (max_jerk[E_AXIS] * 0.5 < fabs(current_speed[E_AXIS])) NOMORE(vmax_junction, max_jerk[E_AXIS] * 0.5);
|
1079
|
|
- NOMORE(vmax_junction, block->nominal_speed);
|
1080
|
|
- float safe_speed = vmax_junction;
|
|
1094
|
+ /**
|
|
1095
|
+ * Adapted from Prusa MKS firmware
|
|
1096
|
+ *
|
|
1097
|
+ * Start with a safe speed (from which the machine may halt to stop immediately).
|
|
1098
|
+ */
|
|
1099
|
+
|
|
1100
|
+ // Exit speed limited by a jerk to full halt of a previous last segment
|
|
1101
|
+ static float previous_safe_speed;
|
|
1102
|
+
|
|
1103
|
+ float safe_speed = block->nominal_speed;
|
|
1104
|
+ bool limited = false;
|
|
1105
|
+ LOOP_XYZE(i) {
|
|
1106
|
+ float jerk = fabs(current_speed[i]);
|
|
1107
|
+ if (jerk > max_jerk[i]) {
|
|
1108
|
+ // The actual jerk is lower if it has been limited by the XY jerk.
|
|
1109
|
+ if (limited) {
|
|
1110
|
+ // Spare one division by a following gymnastics:
|
|
1111
|
+ // Instead of jerk *= safe_speed / block->nominal_speed,
|
|
1112
|
+ // multiply max_jerk[i] by the divisor.
|
|
1113
|
+ jerk *= safe_speed;
|
|
1114
|
+ float mjerk = max_jerk[i] * block->nominal_speed;
|
|
1115
|
+ if (jerk > mjerk) safe_speed *= mjerk / jerk;
|
|
1116
|
+ }
|
|
1117
|
+ else {
|
|
1118
|
+ safe_speed = max_jerk[i];
|
|
1119
|
+ limited = true;
|
|
1120
|
+ }
|
|
1121
|
+ }
|
|
1122
|
+ }
|
1081
|
1123
|
|
1082
|
1124
|
if (moves_queued > 1 && previous_nominal_speed > 0.0001) {
|
1083
|
|
- //if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
|
1084
|
|
- vmax_junction = block->nominal_speed;
|
1085
|
|
- //}
|
1086
|
|
-
|
1087
|
|
- float dsx = fabs(current_speed[X_AXIS] - previous_speed[X_AXIS]),
|
1088
|
|
- dsy = fabs(current_speed[Y_AXIS] - previous_speed[Y_AXIS]),
|
1089
|
|
- dsz = fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]),
|
1090
|
|
- dse = fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]);
|
1091
|
|
- if (dsx > max_jerk[X_AXIS]) NOMORE(vmax_junction_factor, max_jerk[X_AXIS] / dsx);
|
1092
|
|
- if (dsy > max_jerk[Y_AXIS]) NOMORE(vmax_junction_factor, max_jerk[Y_AXIS] / dsy);
|
1093
|
|
- if (dsz > max_jerk[Z_AXIS]) NOMORE(vmax_junction_factor, max_jerk[Z_AXIS] / dsz);
|
1094
|
|
- if (dse > max_jerk[E_AXIS]) NOMORE(vmax_junction_factor, max_jerk[E_AXIS] / dse);
|
1095
|
|
-
|
1096
|
|
- vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
|
|
1125
|
+ // Estimate a maximum velocity allowed at a joint of two successive segments.
|
|
1126
|
+ // If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
|
|
1127
|
+ // then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
|
|
1128
|
+
|
|
1129
|
+ // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
|
|
1130
|
+ bool prev_speed_larger = previous_nominal_speed > block->nominal_speed;
|
|
1131
|
+ float smaller_speed_factor = prev_speed_larger ? (block->nominal_speed / previous_nominal_speed) : (previous_nominal_speed / block->nominal_speed);
|
|
1132
|
+ // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
|
|
1133
|
+ vmax_junction = prev_speed_larger ? block->nominal_speed : previous_nominal_speed;
|
|
1134
|
+ // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
|
|
1135
|
+ float v_factor = 1.f;
|
|
1136
|
+ limited = false;
|
|
1137
|
+ // Now limit the jerk in all axes.
|
|
1138
|
+ LOOP_XYZE(axis) {
|
|
1139
|
+ // Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
|
|
1140
|
+ float v_exit = previous_speed[axis], v_entry = current_speed[axis];
|
|
1141
|
+ if (prev_speed_larger) v_exit *= smaller_speed_factor;
|
|
1142
|
+ if (limited) {
|
|
1143
|
+ v_exit *= v_factor;
|
|
1144
|
+ v_entry *= v_factor;
|
|
1145
|
+ }
|
|
1146
|
+ // Calculate jerk depending on whether the axis is coasting in the same direction or reversing.
|
|
1147
|
+ float jerk =
|
|
1148
|
+ (v_exit > v_entry) ?
|
|
1149
|
+ ((v_entry > 0.f || v_exit < 0.f) ?
|
|
1150
|
+ // coasting
|
|
1151
|
+ (v_exit - v_entry) :
|
|
1152
|
+ // axis reversal
|
|
1153
|
+ max(v_exit, -v_entry)) :
|
|
1154
|
+ // v_exit <= v_entry
|
|
1155
|
+ ((v_entry < 0.f || v_exit > 0.f) ?
|
|
1156
|
+ // coasting
|
|
1157
|
+ (v_entry - v_exit) :
|
|
1158
|
+ // axis reversal
|
|
1159
|
+ max(-v_exit, v_entry));
|
|
1160
|
+ if (jerk > max_jerk[axis]) {
|
|
1161
|
+ v_factor *= max_jerk[axis] / jerk;
|
|
1162
|
+ limited = true;
|
|
1163
|
+ }
|
|
1164
|
+ }
|
|
1165
|
+ if (limited) vmax_junction *= v_factor;
|
|
1166
|
+ // Now the transition velocity is known, which maximizes the shared exit / entry velocity while
|
|
1167
|
+ // respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints.
|
|
1168
|
+ float vmax_junction_threshold = vmax_junction * 0.99f;
|
|
1169
|
+ if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold) {
|
|
1170
|
+ // Not coasting. The machine will stop and start the movements anyway,
|
|
1171
|
+ // better to start the segment from start.
|
|
1172
|
+ block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT;
|
|
1173
|
+ vmax_junction = safe_speed;
|
|
1174
|
+ }
|
1097
|
1175
|
}
|
|
1176
|
+ else {
|
|
1177
|
+ block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT;
|
|
1178
|
+ vmax_junction = safe_speed;
|
|
1179
|
+ }
|
|
1180
|
+
|
|
1181
|
+ // Max entry speed of this block equals the max exit speed of the previous block.
|
1098
|
1182
|
block->max_entry_speed = vmax_junction;
|
1099
|
1183
|
|
1100
|
1184
|
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
|
|
@@ -1109,13 +1193,12 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
|
1109
|
1193
|
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
|
1110
|
1194
|
// the reverse and forward planners, the corresponding block junction speed will always be at the
|
1111
|
1195
|
// the maximum junction speed and may always be ignored for any speed reduction checks.
|
1112
|
|
- block->flag &= ~BLOCK_FLAG_NOMINAL_LENGTH;
|
1113
|
|
- if (block->nominal_speed <= v_allowable) block->flag |= BLOCK_FLAG_NOMINAL_LENGTH;
|
1114
|
|
- block->flag |= BLOCK_FLAG_RECALCULATE; // Always calculate trapezoid for new block
|
|
1196
|
+ block->flag |= BLOCK_FLAG_RECALCULATE | (block->nominal_speed <= v_allowable ? BLOCK_FLAG_NOMINAL_LENGTH : 0);
|
1115
|
1197
|
|
1116
|
1198
|
// Update previous path unit_vector and nominal speed
|
1117
|
1199
|
memcpy(previous_speed, current_speed, sizeof(previous_speed));
|
1118
|
1200
|
previous_nominal_speed = block->nominal_speed;
|
|
1201
|
+ previous_safe_speed = safe_speed;
|
1119
|
1202
|
|
1120
|
1203
|
#if ENABLED(LIN_ADVANCE)
|
1121
|
1204
|
|