⚡️ Optimize G2-G3 Arcs (#24366)
This commit is contained in:
parent
79a332b57e
commit
0c78a6f657
@ -374,11 +374,12 @@
|
||||
#endif
|
||||
|
||||
NOLESS(segments, 1U); // Must have at least one segment
|
||||
const float inv_segments = 1.0f / segments, // Reciprocal to save calculation
|
||||
segment_xyz_mm = SQRT(cart_xy_mm_2 + sq(total.z)) * inv_segments; // Length of each segment
|
||||
const float inv_segments = 1.0f / segments; // Reciprocal to save calculation
|
||||
|
||||
// Add hints to help optimize the move
|
||||
PlannerHints hints(SQRT(cart_xy_mm_2 + sq(total.z)) * inv_segments); // Length of each segment
|
||||
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
||||
const float inv_duration = scaled_fr_mm_s / segment_xyz_mm;
|
||||
hints.inv_duration = scaled_fr_mm_s / hints.millimeters;
|
||||
#endif
|
||||
|
||||
xyze_float_t diff = total * inv_segments;
|
||||
@ -392,13 +393,9 @@
|
||||
if (!planner.leveling_active || !planner.leveling_active_at_z(destination.z)) {
|
||||
while (--segments) {
|
||||
raw += diff;
|
||||
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm
|
||||
OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
|
||||
);
|
||||
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);
|
||||
}
|
||||
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, segment_xyz_mm
|
||||
OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
|
||||
);
|
||||
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, hints);
|
||||
return false; // Did not set current from destination
|
||||
}
|
||||
|
||||
@ -467,7 +464,7 @@
|
||||
TERN_(ENABLE_LEVELING_FADE_HEIGHT, * fade_scaling_factor); // apply fade factor to interpolated height
|
||||
|
||||
const float oldz = raw.z; raw.z += z_cxcy;
|
||||
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration) );
|
||||
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);
|
||||
raw.z = oldz;
|
||||
|
||||
if (segments == 0) // done with last segment
|
||||
|
@ -172,8 +172,9 @@ Joystick joystick;
|
||||
current_position += move_dist;
|
||||
apply_motion_limits(current_position);
|
||||
const float length = sqrt(hypot2);
|
||||
PlannerHints hints(length);
|
||||
injecting_now = true;
|
||||
planner.buffer_line(current_position, length / seg_time, active_extruder, length);
|
||||
planner.buffer_line(current_position, length / seg_time, active_extruder, hints);
|
||||
injecting_now = false;
|
||||
}
|
||||
}
|
||||
|
@ -214,9 +214,12 @@ void plan_arc(
|
||||
const uint16_t segments = nominal_segment_mm > (MAX_ARC_SEGMENT_MM) ? CEIL(flat_mm / (MAX_ARC_SEGMENT_MM)) :
|
||||
nominal_segment_mm < (MIN_ARC_SEGMENT_MM) ? _MAX(1, FLOOR(flat_mm / (MIN_ARC_SEGMENT_MM))) :
|
||||
nominal_segments;
|
||||
const float segment_mm = flat_mm / segments;
|
||||
|
||||
// Add hints to help optimize the move
|
||||
PlannerHints hints;
|
||||
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
||||
const float inv_duration = (scaled_fr_mm_s / flat_mm) * segments;
|
||||
hints.inv_duration = (scaled_fr_mm_s / flat_mm) * segments;
|
||||
#endif
|
||||
|
||||
/**
|
||||
@ -288,6 +291,16 @@ void plan_arc(
|
||||
int8_t arc_recalc_count = N_ARC_CORRECTION;
|
||||
#endif
|
||||
|
||||
// An arc can always complete within limits from a speed which...
|
||||
// a) is <= any configured maximum speed,
|
||||
// b) does not require centripetal force greater than any configured maximum acceleration,
|
||||
// c) allows the print head to stop in the remining length of the curve within all configured maximum accelerations.
|
||||
// The last has to be calculated every time through the loop.
|
||||
const float limiting_accel = _MIN(planner.settings.max_acceleration_mm_per_s2[axis_p], planner.settings.max_acceleration_mm_per_s2[axis_q]),
|
||||
limiting_speed = _MIN(planner.settings.max_feedrate_mm_s[axis_p], planner.settings.max_acceleration_mm_per_s2[axis_q]),
|
||||
limiting_speed_sqr = _MIN(sq(limiting_speed), limiting_accel * radius);
|
||||
float arc_mm_remaining = flat_mm;
|
||||
|
||||
for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
|
||||
|
||||
thermalManager.task();
|
||||
@ -342,7 +355,13 @@ void plan_arc(
|
||||
planner.apply_leveling(raw);
|
||||
#endif
|
||||
|
||||
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)))
|
||||
// calculate safe speed for stopping by the end of the arc
|
||||
arc_mm_remaining -= segment_mm;
|
||||
|
||||
hints.curve_radius = i > 1 ? radius : 0;
|
||||
hints.safe_exit_speed_sqr = _MIN(limiting_speed_sqr, 2 * limiting_accel * arc_mm_remaining);
|
||||
|
||||
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints))
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -363,7 +382,8 @@ void plan_arc(
|
||||
planner.apply_leveling(raw);
|
||||
#endif
|
||||
|
||||
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration));
|
||||
hints.curve_radius = 0;
|
||||
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);
|
||||
|
||||
#if ENABLED(AUTO_BED_LEVELING_UBL)
|
||||
ARC_LIJKUVW_CODE(
|
||||
|
@ -1041,19 +1041,18 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
|
||||
NOLESS(segments, 1U);
|
||||
|
||||
// The approximate length of each segment
|
||||
const float inv_segments = 1.0f / float(segments),
|
||||
cartesian_segment_mm = cartesian_mm * inv_segments;
|
||||
const float inv_segments = 1.0f / float(segments);
|
||||
const xyze_float_t segment_distance = diff * inv_segments;
|
||||
|
||||
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
||||
const float inv_duration = scaled_fr_mm_s / cartesian_segment_mm;
|
||||
#endif
|
||||
// Add hints to help optimize the move
|
||||
PlannerHints hints(cartesian_mm * inv_segments);
|
||||
TERN_(SCARA_FEEDRATE_SCALING, hints.inv_duration = scaled_fr_mm_s / hints.millimeters);
|
||||
|
||||
/*
|
||||
SERIAL_ECHOPGM("mm=", cartesian_mm);
|
||||
SERIAL_ECHOPGM(" seconds=", seconds);
|
||||
SERIAL_ECHOPGM(" segments=", segments);
|
||||
SERIAL_ECHOPGM(" segment_mm=", cartesian_segment_mm);
|
||||
SERIAL_ECHOPGM(" segment_mm=", hints.millimeters);
|
||||
SERIAL_EOL();
|
||||
//*/
|
||||
|
||||
@ -1065,11 +1064,12 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
|
||||
while (--segments) {
|
||||
segment_idle(next_idle_ms);
|
||||
raw += segment_distance;
|
||||
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, cartesian_segment_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration))) break;
|
||||
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints))
|
||||
break;
|
||||
}
|
||||
|
||||
// Ensure last segment arrives at target location.
|
||||
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, cartesian_segment_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration));
|
||||
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, hints);
|
||||
|
||||
return false; // caller will update current_position
|
||||
}
|
||||
@ -1108,17 +1108,16 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
|
||||
NOLESS(segments, 1U);
|
||||
|
||||
// The approximate length of each segment
|
||||
const float inv_segments = 1.0f / float(segments),
|
||||
cartesian_segment_mm = cartesian_mm * inv_segments;
|
||||
const float inv_segments = 1.0f / float(segments);
|
||||
const xyze_float_t segment_distance = diff * inv_segments;
|
||||
|
||||
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
||||
const float inv_duration = scaled_fr_mm_s / cartesian_segment_mm;
|
||||
#endif
|
||||
// Add hints to help optimize the move
|
||||
PlannerHints hints(cartesian_mm * inv_segments);
|
||||
TERN_(SCARA_FEEDRATE_SCALING, hints.inv_duration = scaled_fr_mm_s / hints.millimeters);
|
||||
|
||||
//SERIAL_ECHOPGM("mm=", cartesian_mm);
|
||||
//SERIAL_ECHOLNPGM(" segments=", segments);
|
||||
//SERIAL_ECHOLNPGM(" segment_mm=", cartesian_segment_mm);
|
||||
//SERIAL_ECHOLNPGM(" segment_mm=", hints.millimeters);
|
||||
|
||||
// Get the raw current position as starting point
|
||||
xyze_pos_t raw = current_position;
|
||||
@ -1128,12 +1127,13 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
|
||||
while (--segments) {
|
||||
segment_idle(next_idle_ms);
|
||||
raw += segment_distance;
|
||||
if (!planner.buffer_line(raw, fr_mm_s, active_extruder, cartesian_segment_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration))) break;
|
||||
if (!planner.buffer_line(raw, fr_mm_s, active_extruder, hints))
|
||||
break;
|
||||
}
|
||||
|
||||
// Since segment_distance is only approximate,
|
||||
// the final move must be to the exact destination.
|
||||
planner.buffer_line(destination, fr_mm_s, active_extruder, cartesian_segment_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration));
|
||||
planner.buffer_line(destination, fr_mm_s, active_extruder, hints);
|
||||
}
|
||||
|
||||
#endif // SEGMENT_LEVELED_MOVES && !AUTO_BED_LEVELING_UBL
|
||||
|
@ -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()
|
||||
|
||||
|
@ -280,6 +280,8 @@ typedef struct block_t {
|
||||
block_laser_t laser;
|
||||
#endif
|
||||
|
||||
void reset() { memset((char*)this, 0, sizeof(*this)); }
|
||||
|
||||
} block_t;
|
||||
|
||||
#if ANY(LIN_ADVANCE, SCARA_FEEDRATE_SCALING, GRADIENT_MIX, LCD_SHOW_E_TOTAL, POWER_LOSS_RECOVERY)
|
||||
@ -349,6 +351,30 @@ typedef struct {
|
||||
typedef IF<(BLOCK_BUFFER_SIZE > 64), uint16_t, uint8_t>::type last_move_t;
|
||||
#endif
|
||||
|
||||
#if ENABLED(ARC_SUPPORT)
|
||||
#define HINTS_CURVE_RADIUS
|
||||
#define HINTS_SAFE_EXIT_SPEED
|
||||
#endif
|
||||
|
||||
struct PlannerHints {
|
||||
float millimeters = 0.0; // Move Length, if known, else 0.
|
||||
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
||||
float inv_duration = 0.0; // Reciprocal of the move duration, if known
|
||||
#endif
|
||||
#if ENABLED(HINTS_CURVE_RADIUS)
|
||||
float curve_radius = 0.0; // Radius of curvature of the motion path - to calculate cornering speed
|
||||
#else
|
||||
static constexpr float curve_radius = 0.0;
|
||||
#endif
|
||||
#if ENABLED(HINTS_SAFE_EXIT_SPEED)
|
||||
float safe_exit_speed_sqr = 0.0; // Square of the speed considered "safe" at the end of the segment
|
||||
// i.e., at or below the exit speed of the segment that the planner
|
||||
// would calculate if it knew the as-yet-unbuffered path
|
||||
#endif
|
||||
|
||||
PlannerHints(const_float_t mm=0.0f) : millimeters(mm) {}
|
||||
};
|
||||
|
||||
class Planner {
|
||||
public:
|
||||
|
||||
@ -752,14 +778,14 @@ class Planner {
|
||||
* target - target position in steps units
|
||||
* fr_mm_s - (target) speed of the move
|
||||
* extruder - target extruder
|
||||
* millimeters - the length of the movement, if known
|
||||
* hints - parameters to aid planner calculations
|
||||
*
|
||||
* Returns true if movement was buffered, false otherwise
|
||||
*/
|
||||
static bool _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=0.0
|
||||
, feedRate_t fr_mm_s, const uint8_t extruder, const PlannerHints &hints
|
||||
);
|
||||
|
||||
/**
|
||||
@ -774,15 +800,14 @@ class Planner {
|
||||
* @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
|
||||
*/
|
||||
static bool _populate_block(block_t * const block, 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=0.0
|
||||
, feedRate_t fr_mm_s, const uint8_t extruder, const PlannerHints &hints
|
||||
);
|
||||
|
||||
/**
|
||||
@ -809,12 +834,14 @@ class Planner {
|
||||
*
|
||||
* 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
|
||||
* extruder - optional target extruder (otherwise active_extruder)
|
||||
* hints - optional parameters to aid planner calculations
|
||||
*/
|
||||
static bool 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()
|
||||
);
|
||||
|
||||
public:
|
||||
@ -826,12 +853,12 @@ class Planner {
|
||||
*
|
||||
* 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
|
||||
*/
|
||||
static bool 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)
|
||||
static bool buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s
|
||||
, const uint8_t extruder=active_extruder
|
||||
, const PlannerHints &hints=PlannerHints()
|
||||
);
|
||||
|
||||
#if ENABLED(DIRECT_STEPPING)
|
||||
@ -1024,15 +1051,15 @@ class Planner {
|
||||
|
||||
static void calculate_trapezoid_for_block(block_t * const block, const_float_t entry_factor, const_float_t exit_factor);
|
||||
|
||||
static void reverse_pass_kernel(block_t * const current, const block_t * const next);
|
||||
static void reverse_pass_kernel(block_t * const current, const block_t * const next OPTARG(ARC_SUPPORT, const_float_t safe_exit_speed_sqr));
|
||||
static void forward_pass_kernel(const block_t * const previous, block_t * const current, uint8_t block_index);
|
||||
|
||||
static void reverse_pass();
|
||||
static void reverse_pass(TERN_(ARC_SUPPORT, const_float_t safe_exit_speed_sqr));
|
||||
static void forward_pass();
|
||||
|
||||
static void recalculate_trapezoids();
|
||||
static void recalculate_trapezoids(TERN_(ARC_SUPPORT, const_float_t safe_exit_speed_sqr));
|
||||
|
||||
static void recalculate();
|
||||
static void recalculate(TERN_(ARC_SUPPORT, const_float_t safe_exit_speed_sqr));
|
||||
|
||||
#if HAS_JUNCTION_DEVIATION
|
||||
|
||||
|
@ -121,6 +121,9 @@ void cubic_b_spline(
|
||||
|
||||
millis_t next_idle_ms = millis() + 200UL;
|
||||
|
||||
// Hints to help optimize the move
|
||||
PlannerHints hints;
|
||||
|
||||
for (float t = 0; t < 1;) {
|
||||
|
||||
thermalManager.task();
|
||||
@ -177,7 +180,7 @@ void cubic_b_spline(
|
||||
}
|
||||
*/
|
||||
|
||||
step = new_t - t;
|
||||
hints.millimeters = new_t - t;
|
||||
t = new_t;
|
||||
|
||||
// Compute and send new position
|
||||
@ -203,7 +206,7 @@ void cubic_b_spline(
|
||||
const xyze_pos_t &pos = bez_target;
|
||||
#endif
|
||||
|
||||
if (!planner.buffer_line(pos, scaled_fr_mm_s, active_extruder, step))
|
||||
if (!planner.buffer_line(pos, scaled_fr_mm_s, active_extruder, hints))
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
@ -2002,14 +2002,15 @@ uint32_t Stepper::block_phase_isr() {
|
||||
else if (LA_steps) nextAdvanceISR = 0;
|
||||
#endif
|
||||
|
||||
/*
|
||||
/**
|
||||
* Adjust Laser Power - Accelerating
|
||||
* isPowered - True when a move is powered.
|
||||
* isEnabled - laser power is active.
|
||||
* Laser power variables are calulated and stored in this block by the planner code.
|
||||
*
|
||||
* trap_ramp_active_pwr - the active power in this block across accel or decel trap steps.
|
||||
* trap_ramp_entry_incr - holds the precalculated value to increase the current power per accel step.
|
||||
* isPowered - True when a move is powered.
|
||||
* isEnabled - laser power is active.
|
||||
*
|
||||
* Laser power variables are calulated and stored in this block by the planner code.
|
||||
* trap_ramp_active_pwr - the active power in this block across accel or decel trap steps.
|
||||
* trap_ramp_entry_incr - holds the precalculated value to increase the current power per accel step.
|
||||
*
|
||||
* Apply the starting active power and then increase power per step by the trap_ramp_entry_incr value if positive.
|
||||
*/
|
||||
@ -2032,6 +2033,7 @@ uint32_t Stepper::block_phase_isr() {
|
||||
uint32_t step_rate;
|
||||
|
||||
#if ENABLED(S_CURVE_ACCELERATION)
|
||||
|
||||
// If this is the 1st time we process the 2nd half of the trapezoid...
|
||||
if (!bezier_2nd_half) {
|
||||
// Initialize the Bézier speed curve
|
||||
@ -2046,6 +2048,7 @@ uint32_t Stepper::block_phase_isr() {
|
||||
? _eval_bezier_curve(deceleration_time)
|
||||
: current_block->final_rate;
|
||||
}
|
||||
|
||||
#else
|
||||
// Using the old trapezoidal control
|
||||
step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate);
|
||||
@ -2055,9 +2058,8 @@ uint32_t Stepper::block_phase_isr() {
|
||||
}
|
||||
else
|
||||
step_rate = current_block->final_rate;
|
||||
#endif
|
||||
|
||||
// step_rate is in steps/second
|
||||
#endif
|
||||
|
||||
// step_rate to timer interval and steps per stepper isr
|
||||
interval = calc_timer_interval(step_rate, &steps_per_isr);
|
||||
@ -2109,10 +2111,10 @@ uint32_t Stepper::block_phase_isr() {
|
||||
interval = ticks_nominal;
|
||||
}
|
||||
|
||||
/* Adjust Laser Power - Cruise
|
||||
/**
|
||||
* Adjust Laser Power - Cruise
|
||||
* power - direct or floor adjusted active laser power.
|
||||
*/
|
||||
|
||||
#if ENABLED(LASER_POWER_TRAP)
|
||||
if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) {
|
||||
if (step_events_completed + 1 == accelerate_until) {
|
||||
@ -2130,7 +2132,7 @@ uint32_t Stepper::block_phase_isr() {
|
||||
}
|
||||
|
||||
#if ENABLED(LASER_FEATURE)
|
||||
/*
|
||||
/**
|
||||
* CUTTER_MODE_DYNAMIC is experimental and developing.
|
||||
* Super-fast method to dynamically adjust the laser power OCR value based on the input feedrate in mm-per-minute.
|
||||
* TODO: Set up Min/Max OCR offsets to allow tuning and scaling of various lasers.
|
||||
@ -2147,9 +2149,8 @@ uint32_t Stepper::block_phase_isr() {
|
||||
}
|
||||
else { // !current_block
|
||||
#if ENABLED(LASER_FEATURE)
|
||||
if (cutter.cutter_mode == CUTTER_MODE_DYNAMIC) {
|
||||
if (cutter.cutter_mode == CUTTER_MODE_DYNAMIC)
|
||||
cutter.apply_power(0); // No movement in dynamic mode so turn Laser off
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user