MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

⚡️ Optimize G2-G3 Arcs (#24366)

Browse Source
This commit is contained in:
tombrazier 2022-07-08 20:41:39 +01:00 committed by Scott Lahteine
parent 79a332b57e
commit 0c78a6f657
8 changed files with 310 additions and 255 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
View File

@ -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

3
Marlin/src/feature/joystick.cpp
View File

@ -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;
}
}

26
Marlin/src/gcode/motion/G2_G3.cpp
View File

@ -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(

32
Marlin/src/module/motion.cpp
View File

@ -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

392
Marlin/src/module/planner.cpp
View File

@ -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()

61
Marlin/src/module/planner.h
View File

@ -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

7
Marlin/src/module/planner_bezier.cpp
View File

@ -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;
}
}

27
Marlin/src/module/stepper.cpp
View File

@ -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
}