⚡️ Optimize G2-G3 Arcs (#24366)
This commit is contained in:
tombrazier
committed by
Scott Lahteine
Scott Lahteine
parent
79a332b57e
commit
0c78a6f657
@ -843,20 +843,22 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
|
||||
/**
|
||||
* Laser Trapezoid Calculations
|
||||
*
|
||||
* Approximate the trapezoid with the laser, incrementing the power every `trap_ramp_entry_incr` steps while accelerating,
|
||||
* and decrementing the power every `trap_ramp_exit_decr` while decelerating, to keep power proportional to feedrate.
|
||||
* Laser power trap will reduce the initial power to no less than the laser_power_floor value. Based on the number
|
||||
* of calculated accel/decel steps the power is distributed over the trapezoid entry- and exit-ramp steps.
|
||||
* Approximate the trapezoid with the laser, incrementing the power every `trap_ramp_entry_incr`
|
||||
* steps while accelerating, and decrementing the power every `trap_ramp_exit_decr` while decelerating,
|
||||
* to keep power proportional to feedrate. Laser power trap will reduce the initial power to no less
|
||||
* than the laser_power_floor value. Based on the number of calculated accel/decel steps the power is
|
||||
* distributed over the trapezoid entry- and exit-ramp steps.
|
||||
*
|
||||
* trap_ramp_active_pwr - The active power is initially set at a reduced level factor of initial power / accel steps and
|
||||
* will be additively incremented using a trap_ramp_entry_incr value for each accel step processed later in the stepper code.
|
||||
* The trap_ramp_exit_decr value is calculated as power / decel steps and is also adjusted to no less than the power floor.
|
||||
* trap_ramp_active_pwr - The active power is initially set at a reduced level factor of initial
|
||||
* power / accel steps and will be additively incremented using a trap_ramp_entry_incr value for each
|
||||
* accel step processed later in the stepper code. The trap_ramp_exit_decr value is calculated as
|
||||
* power / decel steps and is also adjusted to no less than the power floor.
|
||||
*
|
||||
* If the power == 0 the inline mode variables need to be set to zero to prevent stepper processing. The method allows
|
||||
* for simpler non-powered moves like G0 or G28.
|
||||
* If the power == 0 the inline mode variables need to be set to zero to prevent stepper processing.
|
||||
* The method allows for simpler non-powered moves like G0 or G28.
|
||||
*
|
||||
* Laser Trap Power works for all Jerk and Curve modes; however Arc-based moves will have issues since the segments are
|
||||
* usually too small.
|
||||
* Laser Trap Power works for all Jerk and Curve modes; however Arc-based moves will have issues since
|
||||
* the segments are usually too small.
|
||||
*/
|
||||
if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) {
|
||||
if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) {
|
||||
@ -937,20 +939,30 @@ void Planner::calculate_trapezoid_for_block(block_t * const block, const_float_t
|
||||
this block can never be less than block_buffer_tail and will always be pushed forward and maintain
|
||||
this requirement when encountered by the Planner::release_current_block() routine during a cycle.
|
||||
|
||||
NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short
|
||||
line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't
|
||||
enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then
|
||||
decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and
|
||||
becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner
|
||||
will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line
|
||||
motion(s) distance per block to a desired tolerance. The more combined distance the planner has to use,
|
||||
the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance
|
||||
for the planner to compute over. It also increases the number of computations the planner has to perform
|
||||
to compute an optimal plan, so select carefully.
|
||||
NOTE: Since the planner only computes on what's in the planner buffer, some motions with many short
|
||||
segments (e.g., complex curves) may seem to move slowly. This is because there simply isn't
|
||||
enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and
|
||||
then decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this
|
||||
happens and becomes an annoyance, there are a few simple solutions:
|
||||
|
||||
- Maximize the machine acceleration. The planner will be able to compute higher velocity profiles
|
||||
within the same combined distance.
|
||||
|
||||
- Maximize line motion(s) distance per block to a desired tolerance. The more combined distance the
|
||||
planner has to use, the faster it can go.
|
||||
|
||||
- Maximize the planner buffer size. This also will increase the combined distance for the planner to
|
||||
compute over. It also increases the number of computations the planner has to perform to compute an
|
||||
optimal plan, so select carefully.
|
||||
|
||||
- Use G2/G3 arcs instead of many short segments. Arcs inform the planner of a safe exit speed at the
|
||||
end of the last segment, which alleviates this problem.
|
||||
*/
|
||||
|
||||
// The kernel called by recalculate() when scanning the plan from last to first entry.
|
||||
void Planner::reverse_pass_kernel(block_t * const current, const block_t * const next) {
|
||||
void Planner::reverse_pass_kernel(block_t * const current, const block_t * const next
|
||||
OPTARG(HINTS_SAFE_EXIT_SPEED, const_float_t safe_exit_speed_sqr)
|
||||
) {
|
||||
if (current) {
|
||||
// If entry speed is already at the maximum entry speed, and there was no change of speed
|
||||
// in the next block, there is no need to recheck. Block is cruising and there is no need to
|
||||
@ -970,9 +982,10 @@ void Planner::reverse_pass_kernel(block_t * const current, const block_t * const
|
||||
// the reverse and forward planners, the corresponding block junction speed will always be at the
|
||||
// the maximum junction speed and may always be ignored for any speed reduction checks.
|
||||
|
||||
const float new_entry_speed_sqr = current->flag.nominal_length
|
||||
? max_entry_speed_sqr
|
||||
: _MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next ? next->entry_speed_sqr : sq(float(MINIMUM_PLANNER_SPEED)), current->millimeters));
|
||||
const float next_entry_speed_sqr = next ? next->entry_speed_sqr : _MAX(TERN0(HINTS_SAFE_EXIT_SPEED, safe_exit_speed_sqr), sq(float(MINIMUM_PLANNER_SPEED))),
|
||||
new_entry_speed_sqr = current->flag.nominal_length
|
||||
? max_entry_speed_sqr
|
||||
: _MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next_entry_speed_sqr, current->millimeters));
|
||||
if (current->entry_speed_sqr != new_entry_speed_sqr) {
|
||||
|
||||
// Need to recalculate the block speed - Mark it now, so the stepper
|
||||
@ -1001,7 +1014,7 @@ void Planner::reverse_pass_kernel(block_t * const current, const block_t * const
|
||||
* recalculate() needs to go over the current plan twice.
|
||||
* Once in reverse and once forward. This implements the reverse pass.
|
||||
*/
|
||||
void Planner::reverse_pass() {
|
||||
void Planner::reverse_pass(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t safe_exit_speed_sqr)) {
|
||||
// Initialize block index to the last block in the planner buffer.
|
||||
uint8_t block_index = prev_block_index(block_buffer_head);
|
||||
|
||||
@ -1025,7 +1038,7 @@ void Planner::reverse_pass() {
|
||||
|
||||
// Only process movement blocks
|
||||
if (current->is_move()) {
|
||||
reverse_pass_kernel(current, next);
|
||||
reverse_pass_kernel(current, next OPTARG(HINTS_SAFE_EXIT_SPEED, safe_exit_speed_sqr));
|
||||
next = current;
|
||||
}
|
||||
|
||||
@ -1138,7 +1151,7 @@ void Planner::forward_pass() {
|
||||
* according to the entry_factor for each junction. Must be called by
|
||||
* recalculate() after updating the blocks.
|
||||
*/
|
||||
void Planner::recalculate_trapezoids() {
|
||||
void Planner::recalculate_trapezoids(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t safe_exit_speed_sqr)) {
|
||||
// The tail may be changed by the ISR so get a local copy.
|
||||
uint8_t block_index = block_buffer_tail,
|
||||
head_block_index = block_buffer_head;
|
||||
@ -1211,8 +1224,10 @@ void Planner::recalculate_trapezoids() {
|
||||
block_index = next_block_index(block_index);
|
||||
}
|
||||
|
||||
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
|
||||
if (next) {
|
||||
// Last/newest block in buffer. Always recalculated.
|
||||
if (block) {
|
||||
// Exit speed is set with MINIMUM_PLANNER_SPEED unless some code higher up knows better.
|
||||
next_entry_speed = _MAX(TERN0(HINTS_SAFE_EXIT_SPEED, SQRT(safe_exit_speed_sqr)), float(MINIMUM_PLANNER_SPEED));
|
||||
|
||||
// Mark the next(last) block as RECALCULATE, to prevent the Stepper ISR running it.
|
||||
// As the last block is always recalculated here, there is a chance the block isn't
|
||||
@ -1225,33 +1240,33 @@ void Planner::recalculate_trapezoids() {
|
||||
if (!stepper.is_block_busy(block)) {
|
||||
// Block is not BUSY, we won the race against the Stepper ISR:
|
||||
|
||||
const float next_nominal_speed = SQRT(next->nominal_speed_sqr),
|
||||
nomr = 1.0f / next_nominal_speed;
|
||||
calculate_trapezoid_for_block(next, next_entry_speed * nomr, float(MINIMUM_PLANNER_SPEED) * nomr);
|
||||
const float current_nominal_speed = SQRT(block->nominal_speed_sqr),
|
||||
nomr = 1.0f / current_nominal_speed;
|
||||
calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr);
|
||||
#if ENABLED(LIN_ADVANCE)
|
||||
if (next->use_advance_lead) {
|
||||
const float comp = next->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS];
|
||||
next->max_adv_steps = next_nominal_speed * comp;
|
||||
next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp;
|
||||
if (block->use_advance_lead) {
|
||||
const float comp = block->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS];
|
||||
block->max_adv_steps = current_nominal_speed * comp;
|
||||
block->final_adv_steps = next_entry_speed * comp;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// Reset next only to ensure its trapezoid is computed - The stepper is free to use
|
||||
// Reset block to ensure its trapezoid is computed - The stepper is free to use
|
||||
// the block from now on.
|
||||
next->flag.recalculate = false;
|
||||
block->flag.recalculate = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Planner::recalculate() {
|
||||
void Planner::recalculate(TERN_(HINTS_SAFE_EXIT_SPEED, const_float_t safe_exit_speed_sqr)) {
|
||||
// Initialize block index to the last block in the planner buffer.
|
||||
const uint8_t block_index = prev_block_index(block_buffer_head);
|
||||
// If there is just one block, no planning can be done. Avoid it!
|
||||
if (block_index != block_buffer_planned) {
|
||||
reverse_pass();
|
||||
reverse_pass(TERN_(HINTS_SAFE_EXIT_SPEED, safe_exit_speed_sqr));
|
||||
forward_pass();
|
||||
}
|
||||
recalculate_trapezoids();
|
||||
recalculate_trapezoids(TERN_(HINTS_SAFE_EXIT_SPEED, safe_exit_speed_sqr));
|
||||
}
|
||||
|
||||
/**
|
||||
@ -1777,22 +1792,21 @@ float Planner::get_axis_position_mm(const AxisEnum axis) {
|
||||
void Planner::synchronize() { while (busy()) idle(); }
|
||||
|
||||
/**
|
||||
* Planner::_buffer_steps
|
||||
* @brief Add a new linear movement to the planner queue (in terms of steps).
|
||||
*
|
||||
* Add a new linear movement to the planner queue (in terms of steps).
|
||||
* @param target Target position in steps units
|
||||
* @param target_float Target position in direct (mm, degrees) units.
|
||||
* @param cart_dist_mm The pre-calculated move lengths for all axes, in mm
|
||||
* @param fr_mm_s (target) speed of the move
|
||||
* @param extruder target extruder
|
||||
* @param hints parameters to aid planner calculations
|
||||
*
|
||||
* target - target position in steps units
|
||||
* target_float - target position in direct (mm, degrees) units. optional
|
||||
* fr_mm_s - (target) speed of the move
|
||||
* extruder - target extruder
|
||||
* millimeters - the length of the movement, if known
|
||||
*
|
||||
* Returns true if movement was properly queued, false otherwise (if cleaning)
|
||||
* @return true if movement was properly queued, false otherwise (if cleaning)
|
||||
*/
|
||||
bool Planner::_buffer_steps(const xyze_long_t &target
|
||||
OPTARG(HAS_POSITION_FLOAT, const xyze_pos_t &target_float)
|
||||
OPTARG(HAS_DIST_MM_ARG, const xyze_float_t &cart_dist_mm)
|
||||
, feedRate_t fr_mm_s, const uint8_t extruder, const_float_t millimeters
|
||||
, feedRate_t fr_mm_s, const uint8_t extruder, const PlannerHints &hints
|
||||
) {
|
||||
|
||||
// Wait for the next available block
|
||||
@ -1808,7 +1822,7 @@ bool Planner::_buffer_steps(const xyze_long_t &target
|
||||
if (!_populate_block(block, target
|
||||
OPTARG(HAS_POSITION_FLOAT, target_float)
|
||||
OPTARG(HAS_DIST_MM_ARG, cart_dist_mm)
|
||||
, fr_mm_s, extruder, millimeters
|
||||
, fr_mm_s, extruder, hints
|
||||
)
|
||||
) {
|
||||
// Movement was not queued, probably because it was too short.
|
||||
@ -1830,7 +1844,7 @@ bool Planner::_buffer_steps(const xyze_long_t &target
|
||||
block_buffer_head = next_buffer_head;
|
||||
|
||||
// Recalculate and optimize trapezoidal speed profiles
|
||||
recalculate();
|
||||
recalculate(TERN_(HINTS_SAFE_EXIT_SPEED, hints.safe_exit_speed_sqr));
|
||||
|
||||
// Movement successfully queued!
|
||||
return true;
|
||||
@ -1848,8 +1862,7 @@ bool Planner::_buffer_steps(const xyze_long_t &target
|
||||
* @param cart_dist_mm The pre-calculated move lengths for all axes, in mm
|
||||
* @param fr_mm_s (target) speed of the move
|
||||
* @param extruder target extruder
|
||||
* @param millimeters A pre-calculated linear distance for the move, in mm,
|
||||
* or 0.0 to have the distance calculated here.
|
||||
* @param hints parameters to aid planner calculations
|
||||
*
|
||||
* @return true if movement is acceptable, false otherwise
|
||||
*/
|
||||
@ -1858,7 +1871,7 @@ bool Planner::_populate_block(
|
||||
const abce_long_t &target
|
||||
OPTARG(HAS_POSITION_FLOAT, const xyze_pos_t &target_float)
|
||||
OPTARG(HAS_DIST_MM_ARG, const xyze_float_t &cart_dist_mm)
|
||||
, feedRate_t fr_mm_s, const uint8_t extruder, const_float_t millimeters/*=0.0*/
|
||||
, feedRate_t fr_mm_s, const uint8_t extruder, const PlannerHints &hints
|
||||
) {
|
||||
int32_t LOGICAL_AXIS_LIST(
|
||||
de = target.e - position.e,
|
||||
@ -2134,8 +2147,8 @@ bool Planner::_populate_block(
|
||||
block->millimeters = TERN0(HAS_EXTRUDERS, ABS(steps_dist_mm.e));
|
||||
}
|
||||
else {
|
||||
if (millimeters)
|
||||
block->millimeters = millimeters;
|
||||
if (hints.millimeters)
|
||||
block->millimeters = hints.millimeters;
|
||||
else {
|
||||
/**
|
||||
* Distance for interpretation of feedrate in accordance with LinuxCNC (the successor of NIST
|
||||
@ -2243,15 +2256,9 @@ bool Planner::_populate_block(
|
||||
|
||||
#if ENABLED(AUTO_POWER_CONTROL)
|
||||
if (NUM_AXIS_GANG(
|
||||
block->steps.x,
|
||||
|| block->steps.y,
|
||||
|| block->steps.z,
|
||||
|| block->steps.i,
|
||||
|| block->steps.j,
|
||||
|| block->steps.k,
|
||||
|| block->steps.u,
|
||||
|| block->steps.v,
|
||||
|| block->steps.w
|
||||
block->steps.x, || block->steps.y, || block->steps.z,
|
||||
|| block->steps.i, || block->steps.j, || block->steps.k,
|
||||
|| block->steps.u, || block->steps.v, || block->steps.w
|
||||
)) powerManager.power_on();
|
||||
#endif
|
||||
|
||||
@ -2562,29 +2569,17 @@ bool Planner::_populate_block(
|
||||
if (block->step_event_count <= acceleration_long_cutoff) {
|
||||
LOGICAL_AXIS_CODE(
|
||||
LIMIT_ACCEL_LONG(E_AXIS, E_INDEX_N(extruder)),
|
||||
LIMIT_ACCEL_LONG(A_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(B_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(C_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(I_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(J_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(K_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(U_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(V_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(W_AXIS, 0)
|
||||
LIMIT_ACCEL_LONG(A_AXIS, 0), LIMIT_ACCEL_LONG(B_AXIS, 0), LIMIT_ACCEL_LONG(C_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(I_AXIS, 0), LIMIT_ACCEL_LONG(J_AXIS, 0), LIMIT_ACCEL_LONG(K_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(U_AXIS, 0), LIMIT_ACCEL_LONG(V_AXIS, 0), LIMIT_ACCEL_LONG(W_AXIS, 0)
|
||||
);
|
||||
}
|
||||
else {
|
||||
LOGICAL_AXIS_CODE(
|
||||
LIMIT_ACCEL_FLOAT(E_AXIS, E_INDEX_N(extruder)),
|
||||
LIMIT_ACCEL_FLOAT(A_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(B_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(C_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(I_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(J_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(K_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(U_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(V_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(W_AXIS, 0)
|
||||
LIMIT_ACCEL_FLOAT(A_AXIS, 0), LIMIT_ACCEL_FLOAT(B_AXIS, 0), LIMIT_ACCEL_FLOAT(C_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(I_AXIS, 0), LIMIT_ACCEL_FLOAT(J_AXIS, 0), LIMIT_ACCEL_FLOAT(K_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(U_AXIS, 0), LIMIT_ACCEL_FLOAT(V_AXIS, 0), LIMIT_ACCEL_FLOAT(W_AXIS, 0)
|
||||
);
|
||||
}
|
||||
}
|
||||
@ -2649,7 +2644,10 @@ bool Planner::_populate_block(
|
||||
#if HAS_DIST_MM_ARG
|
||||
cart_dist_mm
|
||||
#else
|
||||
LOGICAL_AXIS_ARRAY(steps_dist_mm.e, steps_dist_mm.x, steps_dist_mm.y, steps_dist_mm.z, steps_dist_mm.i, steps_dist_mm.j, steps_dist_mm.k, steps_dist_mm.u, steps_dist_mm.v, steps_dist_mm.w)
|
||||
LOGICAL_AXIS_ARRAY(steps_dist_mm.e,
|
||||
steps_dist_mm.x, steps_dist_mm.y, steps_dist_mm.z,
|
||||
steps_dist_mm.i, steps_dist_mm.j, steps_dist_mm.k,
|
||||
steps_dist_mm.u, steps_dist_mm.v, steps_dist_mm.w)
|
||||
#endif
|
||||
;
|
||||
|
||||
@ -2670,7 +2668,7 @@ bool Planner::_populate_block(
|
||||
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
|
||||
float junction_cos_theta = LOGICAL_AXIS_GANG(
|
||||
+ (-prev_unit_vec.e * unit_vec.e),
|
||||
(-prev_unit_vec.x * unit_vec.x),
|
||||
+ (-prev_unit_vec.x * unit_vec.x),
|
||||
+ (-prev_unit_vec.y * unit_vec.y),
|
||||
+ (-prev_unit_vec.z * unit_vec.z),
|
||||
+ (-prev_unit_vec.i * unit_vec.i),
|
||||
@ -2687,104 +2685,110 @@ bool Planner::_populate_block(
|
||||
vmax_junction_sqr = sq(float(MINIMUM_PLANNER_SPEED));
|
||||
}
|
||||
else {
|
||||
NOLESS(junction_cos_theta, -0.999999f); // Check for numerical round-off to avoid divide by zero.
|
||||
|
||||
// Convert delta vector to unit vector
|
||||
xyze_float_t junction_unit_vec = unit_vec - prev_unit_vec;
|
||||
normalize_junction_vector(junction_unit_vec);
|
||||
|
||||
const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec),
|
||||
sin_theta_d2 = SQRT(0.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive.
|
||||
const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec);
|
||||
|
||||
vmax_junction_sqr = junction_acceleration * junction_deviation_mm * sin_theta_d2 / (1.0f - sin_theta_d2);
|
||||
if (TERN0(HINTS_CURVE_RADIUS, hints.curve_radius)) {
|
||||
TERN_(HINTS_CURVE_RADIUS, vmax_junction_sqr = junction_acceleration * hints.curve_radius);
|
||||
}
|
||||
else {
|
||||
NOLESS(junction_cos_theta, -0.999999f); // Check for numerical round-off to avoid divide by zero.
|
||||
|
||||
#if ENABLED(JD_HANDLE_SMALL_SEGMENTS)
|
||||
const float sin_theta_d2 = SQRT(0.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive.
|
||||
|
||||
// For small moves with >135° junction (octagon) find speed for approximate arc
|
||||
if (block->millimeters < 1 && junction_cos_theta < -0.7071067812f) {
|
||||
vmax_junction_sqr = junction_acceleration * junction_deviation_mm * sin_theta_d2 / (1.0f - sin_theta_d2);
|
||||
|
||||
#if ENABLED(JD_USE_MATH_ACOS)
|
||||
#if ENABLED(JD_HANDLE_SMALL_SEGMENTS)
|
||||
|
||||
#error "TODO: Inline maths with the MCU / FPU."
|
||||
// For small moves with >135° junction (octagon) find speed for approximate arc
|
||||
if (block->millimeters < 1 && junction_cos_theta < -0.7071067812f) {
|
||||
|
||||
#elif ENABLED(JD_USE_LOOKUP_TABLE)
|
||||
#if ENABLED(JD_USE_MATH_ACOS)
|
||||
|
||||
// Fast acos approximation (max. error +-0.01 rads)
|
||||
// Based on LUT table and linear interpolation
|
||||
#error "TODO: Inline maths with the MCU / FPU."
|
||||
|
||||
/**
|
||||
* // Generate the JD Lookup Table
|
||||
* constexpr float c = 1.00751495f; // Correction factor to center error around 0
|
||||
* for (int i = 0; i < jd_lut_count - 1; ++i) {
|
||||
* const float x0 = (sq(i) - 1) / sq(i),
|
||||
* y0 = acos(x0) * (i == 0 ? 1 : c),
|
||||
* x1 = i < jd_lut_count - 1 ? 0.5 * x0 + 0.5 : 0.999999f,
|
||||
* y1 = acos(x1) * (i < jd_lut_count - 1 ? c : 1);
|
||||
* jd_lut_k[i] = (y0 - y1) / (x0 - x1);
|
||||
* jd_lut_b[i] = (y1 * x0 - y0 * x1) / (x0 - x1);
|
||||
* }
|
||||
*
|
||||
* // Compute correction factor (Set c to 1.0f first!)
|
||||
* float min = INFINITY, max = -min;
|
||||
* for (float t = 0; t <= 1; t += 0.0003f) {
|
||||
* const float e = acos(t) / approx(t);
|
||||
* if (isfinite(e)) {
|
||||
* if (e < min) min = e;
|
||||
* if (e > max) max = e;
|
||||
* }
|
||||
* }
|
||||
* fprintf(stderr, "%.9gf, ", (min + max) / 2);
|
||||
*/
|
||||
static constexpr int16_t jd_lut_count = 16;
|
||||
static constexpr uint16_t jd_lut_tll = _BV(jd_lut_count - 1);
|
||||
static constexpr int16_t jd_lut_tll0 = __builtin_clz(jd_lut_tll) + 1; // i.e., 16 - jd_lut_count + 1
|
||||
static constexpr float jd_lut_k[jd_lut_count] PROGMEM = {
|
||||
-1.03145837f, -1.30760646f, -1.75205851f, -2.41705704f,
|
||||
-3.37769222f, -4.74888992f, -6.69649887f, -9.45661736f,
|
||||
-13.3640480f, -18.8928222f, -26.7136841f, -37.7754593f,
|
||||
-53.4201813f, -75.5458374f, -106.836761f, -218.532821f };
|
||||
static constexpr float jd_lut_b[jd_lut_count] PROGMEM = {
|
||||
1.57079637f, 1.70887053f, 2.04220939f, 2.62408352f,
|
||||
3.52467871f, 4.85302639f, 6.77020454f, 9.50875854f,
|
||||
13.4009285f, 18.9188995f, 26.7321243f, 37.7885055f,
|
||||
53.4293975f, 75.5523529f, 106.841369f, 218.534011f };
|
||||
#elif ENABLED(JD_USE_LOOKUP_TABLE)
|
||||
|
||||
const float neg = junction_cos_theta < 0 ? -1 : 1,
|
||||
t = neg * junction_cos_theta;
|
||||
// Fast acos approximation (max. error +-0.01 rads)
|
||||
// Based on LUT table and linear interpolation
|
||||
|
||||
const int16_t idx = (t < 0.00000003f) ? 0 : __builtin_clz(uint16_t((1.0f - t) * jd_lut_tll)) - jd_lut_tll0;
|
||||
/**
|
||||
* // Generate the JD Lookup Table
|
||||
* constexpr float c = 1.00751495f; // Correction factor to center error around 0
|
||||
* for (int i = 0; i < jd_lut_count - 1; ++i) {
|
||||
* const float x0 = (sq(i) - 1) / sq(i),
|
||||
* y0 = acos(x0) * (i == 0 ? 1 : c),
|
||||
* x1 = i < jd_lut_count - 1 ? 0.5 * x0 + 0.5 : 0.999999f,
|
||||
* y1 = acos(x1) * (i < jd_lut_count - 1 ? c : 1);
|
||||
* jd_lut_k[i] = (y0 - y1) / (x0 - x1);
|
||||
* jd_lut_b[i] = (y1 * x0 - y0 * x1) / (x0 - x1);
|
||||
* }
|
||||
*
|
||||
* // Compute correction factor (Set c to 1.0f first!)
|
||||
* float min = INFINITY, max = -min;
|
||||
* for (float t = 0; t <= 1; t += 0.0003f) {
|
||||
* const float e = acos(t) / approx(t);
|
||||
* if (isfinite(e)) {
|
||||
* if (e < min) min = e;
|
||||
* if (e > max) max = e;
|
||||
* }
|
||||
* }
|
||||
* fprintf(stderr, "%.9gf, ", (min + max) / 2);
|
||||
*/
|
||||
static constexpr int16_t jd_lut_count = 16;
|
||||
static constexpr uint16_t jd_lut_tll = _BV(jd_lut_count - 1);
|
||||
static constexpr int16_t jd_lut_tll0 = __builtin_clz(jd_lut_tll) + 1; // i.e., 16 - jd_lut_count + 1
|
||||
static constexpr float jd_lut_k[jd_lut_count] PROGMEM = {
|
||||
-1.03145837f, -1.30760646f, -1.75205851f, -2.41705704f,
|
||||
-3.37769222f, -4.74888992f, -6.69649887f, -9.45661736f,
|
||||
-13.3640480f, -18.8928222f, -26.7136841f, -37.7754593f,
|
||||
-53.4201813f, -75.5458374f, -106.836761f, -218.532821f };
|
||||
static constexpr float jd_lut_b[jd_lut_count] PROGMEM = {
|
||||
1.57079637f, 1.70887053f, 2.04220939f, 2.62408352f,
|
||||
3.52467871f, 4.85302639f, 6.77020454f, 9.50875854f,
|
||||
13.4009285f, 18.9188995f, 26.7321243f, 37.7885055f,
|
||||
53.4293975f, 75.5523529f, 106.841369f, 218.534011f };
|
||||
|
||||
float junction_theta = t * pgm_read_float(&jd_lut_k[idx]) + pgm_read_float(&jd_lut_b[idx]);
|
||||
if (neg > 0) junction_theta = RADIANS(180) - junction_theta; // acos(-t)
|
||||
const float neg = junction_cos_theta < 0 ? -1 : 1,
|
||||
t = neg * junction_cos_theta;
|
||||
|
||||
#else
|
||||
const int16_t idx = (t < 0.00000003f) ? 0 : __builtin_clz(uint16_t((1.0f - t) * jd_lut_tll)) - jd_lut_tll0;
|
||||
|
||||
// Fast acos(-t) approximation (max. error +-0.033rad = 1.89°)
|
||||
// Based on MinMax polynomial published by W. Randolph Franklin, see
|
||||
// https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
|
||||
// acos( t) = pi / 2 - asin(x)
|
||||
// acos(-t) = pi - acos(t) ... pi / 2 + asin(x)
|
||||
float junction_theta = t * pgm_read_float(&jd_lut_k[idx]) + pgm_read_float(&jd_lut_b[idx]);
|
||||
if (neg > 0) junction_theta = RADIANS(180) - junction_theta; // acos(-t)
|
||||
|
||||
const float neg = junction_cos_theta < 0 ? -1 : 1,
|
||||
t = neg * junction_cos_theta,
|
||||
asinx = 0.032843707f
|
||||
+ t * (-1.451838349f
|
||||
+ t * ( 29.66153956f
|
||||
+ t * (-131.1123477f
|
||||
+ t * ( 262.8130562f
|
||||
+ t * (-242.7199627f
|
||||
+ t * ( 84.31466202f ) ))))),
|
||||
junction_theta = RADIANS(90) + neg * asinx; // acos(-t)
|
||||
#else
|
||||
|
||||
// NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0.
|
||||
// Fast acos(-t) approximation (max. error +-0.033rad = 1.89°)
|
||||
// Based on MinMax polynomial published by W. Randolph Franklin, see
|
||||
// https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
|
||||
// acos( t) = pi / 2 - asin(x)
|
||||
// acos(-t) = pi - acos(t) ... pi / 2 + asin(x)
|
||||
|
||||
#endif
|
||||
const float neg = junction_cos_theta < 0 ? -1 : 1,
|
||||
t = neg * junction_cos_theta,
|
||||
asinx = 0.032843707f
|
||||
+ t * (-1.451838349f
|
||||
+ t * ( 29.66153956f
|
||||
+ t * (-131.1123477f
|
||||
+ t * ( 262.8130562f
|
||||
+ t * (-242.7199627f
|
||||
+ t * ( 84.31466202f ) ))))),
|
||||
junction_theta = RADIANS(90) + neg * asinx; // acos(-t)
|
||||
|
||||
const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta;
|
||||
NOMORE(vmax_junction_sqr, limit_sqr);
|
||||
}
|
||||
// NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0.
|
||||
|
||||
#endif // JD_HANDLE_SMALL_SEGMENTS
|
||||
#endif
|
||||
|
||||
const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta;
|
||||
NOMORE(vmax_junction_sqr, limit_sqr);
|
||||
}
|
||||
|
||||
#endif // JD_HANDLE_SMALL_SEGMENTS
|
||||
}
|
||||
}
|
||||
|
||||
// Get the lowest speed
|
||||
@ -2944,12 +2948,11 @@ bool Planner::_populate_block(
|
||||
} // _populate_block()
|
||||
|
||||
/**
|
||||
* Planner::buffer_sync_block
|
||||
* Add a block to the buffer that just updates the position
|
||||
* @param sync_flag BLOCK_FLAG_SYNC_FANS & BLOCK_FLAG_LASER_PWR
|
||||
* Supports LASER_SYNCHRONOUS_M106_M107 and LASER_POWER_SYNC power sync block buffer queueing.
|
||||
* @brief Add a block to the buffer that just updates the position
|
||||
* Supports LASER_SYNCHRONOUS_M106_M107 and LASER_POWER_SYNC power sync block buffer queueing.
|
||||
*
|
||||
* @param sync_flag The sync flag to set, determining the type of sync the block will do
|
||||
*/
|
||||
|
||||
void Planner::buffer_sync_block(const BlockFlagBit sync_flag/*=BLOCK_BIT_SYNC_POSITION*/) {
|
||||
|
||||
// Wait for the next available block
|
||||
@ -2957,14 +2960,13 @@ void Planner::buffer_sync_block(const BlockFlagBit sync_flag/*=BLOCK_BIT_SYNC_PO
|
||||
block_t * const block = get_next_free_block(next_buffer_head);
|
||||
|
||||
// Clear block
|
||||
memset(block, 0, sizeof(block_t));
|
||||
block->reset();
|
||||
block->flag.apply(sync_flag);
|
||||
|
||||
block->position = position;
|
||||
#if ENABLED(BACKLASH_COMPENSATION)
|
||||
LOOP_NUM_AXES(axis) block->position[axis] += backlash.get_applied_steps((AxisEnum)axis);
|
||||
#endif
|
||||
|
||||
#if BOTH(HAS_FAN, LASER_SYNCHRONOUS_M106_M107)
|
||||
FANS_LOOP(i) block->fan_speed[i] = thermalManager.fan_speed[i];
|
||||
#endif
|
||||
@ -2991,22 +2993,24 @@ void Planner::buffer_sync_block(const BlockFlagBit sync_flag/*=BLOCK_BIT_SYNC_PO
|
||||
} // buffer_sync_block()
|
||||
|
||||
/**
|
||||
* Planner::buffer_segment
|
||||
* @brief Add a single linear movement
|
||||
*
|
||||
* Add a new linear movement to the buffer in axis units.
|
||||
* @description Add a new linear movement to the buffer in axis units.
|
||||
* Leveling and kinematics should be applied before calling this.
|
||||
*
|
||||
* Leveling and kinematics should be applied ahead of calling this.
|
||||
* @param abce Target position in mm and/or degrees
|
||||
* @param cart_dist_mm The pre-calculated move lengths for all axes, in mm
|
||||
* @param fr_mm_s (target) speed of the move
|
||||
* @param extruder optional target extruder (otherwise active_extruder)
|
||||
* @param hints optional parameters to aid planner calculations
|
||||
*
|
||||
* a,b,c,e - target positions in mm and/or degrees
|
||||
* fr_mm_s - (target) speed of the move
|
||||
* extruder - target extruder
|
||||
* millimeters - the length of the movement, if known
|
||||
*
|
||||
* Return 'false' if no segment was queued due to cleaning, cold extrusion, full queue, etc.
|
||||
* @return false if no segment was queued due to cleaning, cold extrusion, full queue, etc.
|
||||
*/
|
||||
bool Planner::buffer_segment(const abce_pos_t &abce
|
||||
OPTARG(HAS_DIST_MM_ARG, const xyze_float_t &cart_dist_mm)
|
||||
, const_feedRate_t fr_mm_s, const uint8_t extruder/*=active_extruder*/, const_float_t millimeters/*=0.0*/
|
||||
, const_feedRate_t fr_mm_s
|
||||
, const uint8_t extruder/*=active_extruder*/
|
||||
, const PlannerHints &hints/*=PlannerHints()*/
|
||||
) {
|
||||
|
||||
// If we are cleaning, do not accept queuing of movements
|
||||
@ -3112,8 +3116,8 @@ bool Planner::buffer_segment(const abce_pos_t &abce
|
||||
if (!_buffer_steps(target
|
||||
OPTARG(HAS_POSITION_FLOAT, target_float)
|
||||
OPTARG(HAS_DIST_MM_ARG, cart_dist_mm)
|
||||
, fr_mm_s, extruder, millimeters)
|
||||
) return false;
|
||||
, fr_mm_s, extruder, hints
|
||||
)) return false;
|
||||
|
||||
stepper.wake_up();
|
||||
return true;
|
||||
@ -3126,12 +3130,12 @@ bool Planner::buffer_segment(const abce_pos_t &abce
|
||||
*
|
||||
* cart - target position in mm or degrees
|
||||
* fr_mm_s - (target) speed of the move (mm/s)
|
||||
* extruder - target extruder
|
||||
* millimeters - the length of the movement, if known
|
||||
* inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled)
|
||||
* extruder - optional target extruder (otherwise active_extruder)
|
||||
* hints - optional parameters to aid planner calculations
|
||||
*/
|
||||
bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s, const uint8_t extruder/*=active_extruder*/, const float millimeters/*=0.0*/
|
||||
OPTARG(SCARA_FEEDRATE_SCALING, const_float_t inv_duration/*=0.0*/)
|
||||
bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s
|
||||
, const uint8_t extruder/*=active_extruder*/
|
||||
, const PlannerHints &hints/*=PlannerHints()*/
|
||||
) {
|
||||
xyze_pos_t machine = cart;
|
||||
TERN_(HAS_POSITION_MODIFIERS, apply_modifiers(machine));
|
||||
@ -3153,28 +3157,30 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s, cons
|
||||
);
|
||||
#endif
|
||||
|
||||
const float mm = millimeters ?: (cart_dist_mm.x || cart_dist_mm.y) ? cart_dist_mm.magnitude() : TERN0(HAS_Z_AXIS, ABS(cart_dist_mm.z));
|
||||
|
||||
// Cartesian XYZ to kinematic ABC, stored in global 'delta'
|
||||
inverse_kinematics(machine);
|
||||
|
||||
PlannerHints ph = hints;
|
||||
if (!hints.millimeters)
|
||||
ph.millimeters = (cart_dist_mm.x || cart_dist_mm.y) ? cart_dist_mm.magnitude() : TERN0(HAS_Z_AXIS, ABS(cart_dist_mm.z));
|
||||
|
||||
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
||||
// For SCARA scale the feedrate from mm/s to degrees/s
|
||||
// i.e., Complete the angular vector in the given time.
|
||||
const float duration_recip = inv_duration ?: fr_mm_s / mm;
|
||||
const float duration_recip = hints.inv_duration ?: fr_mm_s / ph.millimeters;
|
||||
const xyz_pos_t diff = delta - position_float;
|
||||
const feedRate_t feedrate = diff.magnitude() * duration_recip;
|
||||
#else
|
||||
const feedRate_t feedrate = fr_mm_s;
|
||||
#endif
|
||||
TERN_(HAS_EXTRUDERS, delta.e = machine.e);
|
||||
if (buffer_segment(delta OPTARG(HAS_DIST_MM_ARG, cart_dist_mm), feedrate, extruder, mm)) {
|
||||
if (buffer_segment(delta OPTARG(HAS_DIST_MM_ARG, cart_dist_mm), feedrate, extruder, ph)) {
|
||||
position_cart = cart;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
#else
|
||||
return buffer_segment(machine, fr_mm_s, extruder, millimeters);
|
||||
return buffer_segment(machine, fr_mm_s, extruder, hints);
|
||||
#endif
|
||||
} // buffer_line()
|
||||
|
||||
|
||||
Reference in New Issue
Block a user