|
|
|
@ -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 |
|
|
|
* |
|
|
|
* Add a new linear movement to the planner queue (in terms of steps). |
|
|
|
* @brief Add a new linear movement to the planner queue (in terms of steps). |
|
|
|
* |
|
|
|
* 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 |
|
|
|
* @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 |
|
|
|
* |
|
|
|
* 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.
|
|
|
|
|
|
|
|
vmax_junction_sqr = junction_acceleration * junction_deviation_mm * sin_theta_d2 / (1.0f - sin_theta_d2); |
|
|
|
|
|
|
|
#if ENABLED(JD_HANDLE_SMALL_SEGMENTS) |
|
|
|
|
|
|
|
// For small moves with >135° junction (octagon) find speed for approximate arc
|
|
|
|
if (block->millimeters < 1 && junction_cos_theta < -0.7071067812f) { |
|
|
|
|
|
|
|
#if ENABLED(JD_USE_MATH_ACOS) |
|
|
|
|
|
|
|
#error "TODO: Inline maths with the MCU / FPU." |
|
|
|
|
|
|
|
#elif ENABLED(JD_USE_LOOKUP_TABLE) |
|
|
|
|
|
|
|
// Fast acos approximation (max. error +-0.01 rads)
|
|
|
|
// Based on LUT table and linear interpolation
|
|
|
|
|
|
|
|
/**
|
|
|
|
* // 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 }; |
|
|
|
|
|
|
|
const float neg = junction_cos_theta < 0 ? -1 : 1, |
|
|
|
t = neg * junction_cos_theta; |
|
|
|
|
|
|
|
const int16_t idx = (t < 0.00000003f) ? 0 : __builtin_clz(uint16_t((1.0f - t) * jd_lut_tll)) - jd_lut_tll0; |
|
|
|
|
|
|
|
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)
|
|
|
|
|
|
|
|
#else |
|
|
|
|
|
|
|
// 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)
|
|
|
|
|
|
|
|
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)
|
|
|
|
|
|
|
|
// NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0.
|
|
|
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta; |
|
|
|
NOMORE(vmax_junction_sqr, limit_sqr); |
|
|
|
} |
|
|
|
const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec); |
|
|
|
|
|
|
|
#endif // JD_HANDLE_SMALL_SEGMENTS
|
|
|
|
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.
|
|
|
|
|
|
|
|
const float sin_theta_d2 = SQRT(0.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive.
|
|
|
|
|
|
|
|
vmax_junction_sqr = junction_acceleration * junction_deviation_mm * sin_theta_d2 / (1.0f - sin_theta_d2); |
|
|
|
|
|
|
|
#if ENABLED(JD_HANDLE_SMALL_SEGMENTS) |
|
|
|
|
|
|
|
// For small moves with >135° junction (octagon) find speed for approximate arc
|
|
|
|
if (block->millimeters < 1 && junction_cos_theta < -0.7071067812f) { |
|
|
|
|
|
|
|
#if ENABLED(JD_USE_MATH_ACOS) |
|
|
|
|
|
|
|
#error "TODO: Inline maths with the MCU / FPU." |
|
|
|
|
|
|
|
#elif ENABLED(JD_USE_LOOKUP_TABLE) |
|
|
|
|
|
|
|
// Fast acos approximation (max. error +-0.01 rads)
|
|
|
|
// Based on LUT table and linear interpolation
|
|
|
|
|
|
|
|
/**
|
|
|
|
* // 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 }; |
|
|
|
|
|
|
|
const float neg = junction_cos_theta < 0 ? -1 : 1, |
|
|
|
t = neg * junction_cos_theta; |
|
|
|
|
|
|
|
const int16_t idx = (t < 0.00000003f) ? 0 : __builtin_clz(uint16_t((1.0f - t) * jd_lut_tll)) - jd_lut_tll0; |
|
|
|
|
|
|
|
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)
|
|
|
|
|
|
|
|
#else |
|
|
|
|
|
|
|
// 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)
|
|
|
|
|
|
|
|
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)
|
|
|
|
|
|
|
|
// NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0.
|
|
|
|
|
|
|
|
#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 |
|
|
|
* |
|
|
|
* Add a new linear movement to the buffer in axis units. |
|
|
|
* @brief Add a single linear movement |
|
|
|
* |
|
|
|
* Leveling and kinematics should be applied ahead of calling this. |
|
|
|
* @description Add a new linear movement to the buffer in axis units. |
|
|
|
* Leveling and kinematics should be applied before calling this. |
|
|
|
* |
|
|
|
* 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 |
|
|
|
* @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 |
|
|
|
* |
|
|
|
* 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()
|
|
|
|
|
|
|
|
|
tombrazier
3 years ago
Scott Lahteine