|
@@ -2166,11 +2166,22 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
2166
|
2166
|
}
|
2167
|
2167
|
else {
|
2168
|
2168
|
NOLESS(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero.
|
2169
|
|
- const float sin_theta_d2 = SQRT(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive.
|
2170
|
2169
|
|
2171
|
|
- // TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the
|
2172
|
|
- // two junctions. However, this shouldn't be a significant problem except in extreme circumstances.
|
2173
|
|
- vmax_junction_sqr = (JUNCTION_ACCELERATION * JUNCTION_DEVIATION_MM * sin_theta_d2) / (1.0 - sin_theta_d2);
|
|
2170
|
+ float junction_unit_vec[JD_AXES] = {
|
|
2171
|
+ unit_vec[X_AXIS] - previous_unit_vec[X_AXIS],
|
|
2172
|
+ unit_vec[Y_AXIS] - previous_unit_vec[Y_AXIS],
|
|
2173
|
+ unit_vec[Z_AXIS] - previous_unit_vec[Z_AXIS]
|
|
2174
|
+ #if ENABLED(JUNCTION_DEVIATION_INCLUDE_E)
|
|
2175
|
+ , unit_vec[E_AXIS] - previous_unit_vec[E_AXIS]
|
|
2176
|
+ #endif
|
|
2177
|
+ };
|
|
2178
|
+ // Convert delta vector to unit vector
|
|
2179
|
+ normalize_junction_vector(junction_unit_vec);
|
|
2180
|
+
|
|
2181
|
+ const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec),
|
|
2182
|
+ sin_theta_d2 = SQRT(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive.
|
|
2183
|
+
|
|
2184
|
+ vmax_junction_sqr = (junction_acceleration * JUNCTION_DEVIATION_MM * sin_theta_d2) / (1.0 - sin_theta_d2);
|
2174
|
2185
|
if (block->millimeters < 1.0) {
|
2175
|
2186
|
|
2176
|
2187
|
// Fast acos approximation, minus the error bar to be safe
|
|
@@ -2178,7 +2189,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
2178
|
2189
|
|
2179
|
2190
|
// If angle is greater than 135 degrees (octagon), find speed for approximate arc
|
2180
|
2191
|
if (junction_theta > RADIANS(135)) {
|
2181
|
|
- const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * JUNCTION_ACCELERATION;
|
|
2192
|
+ const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * junction_acceleration;
|
2182
|
2193
|
NOMORE(vmax_junction_sqr, limit_sqr);
|
2183
|
2194
|
}
|
2184
|
2195
|
}
|