bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 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
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 #endif
NOLESS(segments, 1U); // Must have at least one segment NOLESS(segments, 1U); // Must have at least one segment
const float inv_segments = 1.0f / segments, // Reciprocal to save calculation 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
// 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) #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 #endif
xyze_float_t diff = total * inv_segments; xyze_float_t diff = total * inv_segments;
@@ -392,13 +393,9 @@
if (!planner.leveling_active || !planner.leveling_active_at_z(destination.z)) { if (!planner.leveling_active || !planner.leveling_active_at_z(destination.z)) {
while (--segments) { while (--segments) {
raw += diff; raw += diff;
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);
OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
);
} }
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, segment_xyz_mm planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, hints);
OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
);
return false; // Did not set current from destination 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 TERN_(ENABLE_LEVELING_FADE_HEIGHT, * fade_scaling_factor); // apply fade factor to interpolated height
const float oldz = raw.z; raw.z += z_cxcy; 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; raw.z = oldz;
if (segments == 0) // done with last segment 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; current_position += move_dist;
apply_motion_limits(current_position); apply_motion_limits(current_position);
const float length = sqrt(hypot2); const float length = sqrt(hypot2);
PlannerHints hints(length);
injecting_now = true; 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; 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)) : 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_segment_mm < (MIN_ARC_SEGMENT_MM) ? _MAX(1, FLOOR(flat_mm / (MIN_ARC_SEGMENT_MM))) :
nominal_segments; nominal_segments;
const float segment_mm = flat_mm / segments;
// Add hints to help optimize the move
PlannerHints hints;
#if ENABLED(SCARA_FEEDRATE_SCALING) #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 #endif
/** /**
@@ -288,6 +291,16 @@ void plan_arc(
int8_t arc_recalc_count = N_ARC_CORRECTION; int8_t arc_recalc_count = N_ARC_CORRECTION;
#endif #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 for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
thermalManager.task(); thermalManager.task();
@@ -342,7 +355,13 @@ void plan_arc(
planner.apply_leveling(raw); planner.apply_leveling(raw);
#endif #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; break;
} }
} }
@@ -363,7 +382,8 @@ void plan_arc(
planner.apply_leveling(raw); planner.apply_leveling(raw);
#endif #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) #if ENABLED(AUTO_BED_LEVELING_UBL)
ARC_LIJKUVW_CODE( 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); NOLESS(segments, 1U);
// The approximate length of each segment // The approximate length of each segment
const float inv_segments = 1.0f / float(segments), const float inv_segments = 1.0f / float(segments);
cartesian_segment_mm = cartesian_mm * inv_segments;
const xyze_float_t segment_distance = diff * inv_segments; const xyze_float_t segment_distance = diff * inv_segments;
#if ENABLED(SCARA_FEEDRATE_SCALING) // Add hints to help optimize the move
const float inv_duration = scaled_fr_mm_s / cartesian_segment_mm; PlannerHints hints(cartesian_mm * inv_segments);
#endif TERN_(SCARA_FEEDRATE_SCALING, hints.inv_duration = scaled_fr_mm_s / hints.millimeters);
/* /*
SERIAL_ECHOPGM("mm=", cartesian_mm); SERIAL_ECHOPGM("mm=", cartesian_mm);
SERIAL_ECHOPGM(" seconds=", seconds); SERIAL_ECHOPGM(" seconds=", seconds);
SERIAL_ECHOPGM(" segments=", segments); SERIAL_ECHOPGM(" segments=", segments);
SERIAL_ECHOPGM(" segment_mm=", cartesian_segment_mm); SERIAL_ECHOPGM(" segment_mm=", hints.millimeters);
SERIAL_EOL(); SERIAL_EOL();
//*/ //*/
@@ -1065,11 +1064,12 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
while (--segments) { while (--segments) {
segment_idle(next_idle_ms); segment_idle(next_idle_ms);
raw += segment_distance; 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. // 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 return false; // caller will update current_position
} }
@@ -1108,17 +1108,16 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
NOLESS(segments, 1U); NOLESS(segments, 1U);
// The approximate length of each segment // The approximate length of each segment
const float inv_segments = 1.0f / float(segments), const float inv_segments = 1.0f / float(segments);
cartesian_segment_mm = cartesian_mm * inv_segments;
const xyze_float_t segment_distance = diff * inv_segments; const xyze_float_t segment_distance = diff * inv_segments;
#if ENABLED(SCARA_FEEDRATE_SCALING) // Add hints to help optimize the move
const float inv_duration = scaled_fr_mm_s / cartesian_segment_mm; PlannerHints hints(cartesian_mm * inv_segments);
#endif TERN_(SCARA_FEEDRATE_SCALING, hints.inv_duration = scaled_fr_mm_s / hints.millimeters);
//SERIAL_ECHOPGM("mm=", cartesian_mm); //SERIAL_ECHOPGM("mm=", cartesian_mm);
//SERIAL_ECHOLNPGM(" segments=", segments); //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 // Get the raw current position as starting point
xyze_pos_t raw = current_position; xyze_pos_t raw = current_position;
@@ -1128,12 +1127,13 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
while (--segments) { while (--segments) {
segment_idle(next_idle_ms); segment_idle(next_idle_ms);
raw += segment_distance; 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, // Since segment_distance is only approximate,
// the final move must be to the exact destination. // 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 #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 * Laser Trapezoid Calculations
* *
* Approximate the trapezoid with the laser, incrementing the power every `trap_ramp_entry_incr` steps while accelerating, * Approximate the trapezoid with the laser, incrementing the power every `trap_ramp_entry_incr`
* and decrementing the power every `trap_ramp_exit_decr` while decelerating, to keep power proportional to feedrate. * steps while accelerating, and decrementing the power every `trap_ramp_exit_decr` while decelerating,
* Laser power trap will reduce the initial power to no less than the laser_power_floor value. Based on the number * to keep power proportional to feedrate. Laser power trap will reduce the initial power to no less
* of calculated accel/decel steps the power is distributed over the trapezoid entry- and exit-ramp steps. * 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 * trap_ramp_active_pwr - The active power is initially set at a reduced level factor of initial
* will be additively incremented using a trap_ramp_entry_incr value for each accel step processed later in the stepper code. * power / accel steps and will be additively incremented using a trap_ramp_entry_incr value for each
* The trap_ramp_exit_decr value is calculated as power / decel steps and is also adjusted to no less than the power floor. * 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 * If the power == 0 the inline mode variables need to be set to zero to prevent stepper processing.
* for simpler non-powered moves like G0 or G28. * 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 * Laser Trap Power works for all Jerk and Curve modes; however Arc-based moves will have issues since
* usually too small. * the segments are usually too small.
*/ */
if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) {
if (planner.laser_inline.status.isPowered && planner.laser_inline.status.isEnabled) { 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 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. 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 NOTE: Since the planner only computes on what's in the planner buffer, some motions with many short
line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't 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 enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and
decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and then decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this
becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner happens and becomes an annoyance, there are a few simple solutions:
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, - Maximize the machine acceleration. The planner will be able to compute higher velocity profiles
the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance within the same 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. - 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. // 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 (current) {
// If entry speed is already at the maximum entry speed, and there was no change of speed // 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 // 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 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. // the maximum junction speed and may always be ignored for any speed reduction checks.
const float new_entry_speed_sqr = current->flag.nominal_length 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))),
? max_entry_speed_sqr new_entry_speed_sqr = current->flag.nominal_length
: _MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next ? next->entry_speed_sqr : sq(float(MINIMUM_PLANNER_SPEED)), current->millimeters)); ? 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) { if (current->entry_speed_sqr != new_entry_speed_sqr) {
// Need to recalculate the block speed - Mark it now, so the stepper // 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. * recalculate() needs to go over the current plan twice.
* Once in reverse and once forward. This implements the reverse pass. * 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. // Initialize block index to the last block in the planner buffer.
uint8_t block_index = prev_block_index(block_buffer_head); uint8_t block_index = prev_block_index(block_buffer_head);
@@ -1025,7 +1038,7 @@ void Planner::reverse_pass() {
// Only process movement blocks // Only process movement blocks
if (current->is_move()) { 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; next = current;
} }
@@ -1138,7 +1151,7 @@ void Planner::forward_pass() {
* according to the entry_factor for each junction. Must be called by * according to the entry_factor for each junction. Must be called by
* recalculate() after updating the blocks. * 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. // The tail may be changed by the ISR so get a local copy.
uint8_t block_index = block_buffer_tail, uint8_t block_index = block_buffer_tail,
head_block_index = block_buffer_head; head_block_index = block_buffer_head;
@@ -1211,8 +1224,10 @@ void Planner::recalculate_trapezoids() {
block_index = next_block_index(block_index); block_index = next_block_index(block_index);
} }
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. // Last/newest block in buffer. Always recalculated.
if (next) { 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. // 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 // 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)) { if (!stepper.is_block_busy(block)) {
// Block is not BUSY, we won the race against the Stepper ISR: // Block is not BUSY, we won the race against the Stepper ISR:
const float next_nominal_speed = SQRT(next->nominal_speed_sqr), const float current_nominal_speed = SQRT(block->nominal_speed_sqr),
nomr = 1.0f / next_nominal_speed; nomr = 1.0f / current_nominal_speed;
calculate_trapezoid_for_block(next, next_entry_speed * nomr, float(MINIMUM_PLANNER_SPEED) * nomr); calculate_trapezoid_for_block(block, current_entry_speed * nomr, next_entry_speed * nomr);
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
if (next->use_advance_lead) { if (block->use_advance_lead) {
const float comp = next->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS]; const float comp = block->e_D_ratio * extruder_advance_K[active_extruder] * settings.axis_steps_per_mm[E_AXIS];
next->max_adv_steps = next_nominal_speed * comp; block->max_adv_steps = current_nominal_speed * comp;
next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp; block->final_adv_steps = next_entry_speed * comp;
} }
#endif #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. // 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. // Initialize block index to the last block in the planner buffer.
const uint8_t block_index = prev_block_index(block_buffer_head); 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 there is just one block, no planning can be done. Avoid it!
if (block_index != block_buffer_planned) { if (block_index != block_buffer_planned) {
reverse_pass(); reverse_pass(TERN_(HINTS_SAFE_EXIT_SPEED, safe_exit_speed_sqr));
forward_pass(); 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(); } 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 * @return true if movement was properly queued, false otherwise (if cleaning)
* 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)
*/ */
bool Planner::_buffer_steps(const xyze_long_t &target bool Planner::_buffer_steps(const xyze_long_t &target
OPTARG(HAS_POSITION_FLOAT, const xyze_pos_t &target_float) OPTARG(HAS_POSITION_FLOAT, const xyze_pos_t &target_float)
OPTARG(HAS_DIST_MM_ARG, const xyze_float_t &cart_dist_mm) 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 // Wait for the next available block
@@ -1808,7 +1822,7 @@ bool Planner::_buffer_steps(const xyze_long_t &target
if (!_populate_block(block, target if (!_populate_block(block, target
OPTARG(HAS_POSITION_FLOAT, target_float) OPTARG(HAS_POSITION_FLOAT, target_float)
OPTARG(HAS_DIST_MM_ARG, cart_dist_mm) 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. // 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; block_buffer_head = next_buffer_head;
// Recalculate and optimize trapezoidal speed profiles // Recalculate and optimize trapezoidal speed profiles
recalculate(); recalculate(TERN_(HINTS_SAFE_EXIT_SPEED, hints.safe_exit_speed_sqr));
// Movement successfully queued! // Movement successfully queued!
return true; 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 cart_dist_mm The pre-calculated move lengths for all axes, in mm
* @param fr_mm_s (target) speed of the move * @param fr_mm_s (target) speed of the move
* @param extruder target extruder * @param extruder target extruder
* @param millimeters A pre-calculated linear distance for the move, in mm, * @param hints parameters to aid planner calculations
* or 0.0 to have the distance calculated here.
* *
* @return true if movement is acceptable, false otherwise * @return true if movement is acceptable, false otherwise
*/ */
@@ -1858,7 +1871,7 @@ bool Planner::_populate_block(
const abce_long_t &target const abce_long_t &target
OPTARG(HAS_POSITION_FLOAT, const xyze_pos_t &target_float) OPTARG(HAS_POSITION_FLOAT, const xyze_pos_t &target_float)
OPTARG(HAS_DIST_MM_ARG, const xyze_float_t &cart_dist_mm) 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( int32_t LOGICAL_AXIS_LIST(
de = target.e - position.e, de = target.e - position.e,
@@ -2134,8 +2147,8 @@ bool Planner::_populate_block(
block->millimeters = TERN0(HAS_EXTRUDERS, ABS(steps_dist_mm.e)); block->millimeters = TERN0(HAS_EXTRUDERS, ABS(steps_dist_mm.e));
} }
else { else {
if (millimeters) if (hints.millimeters)
block->millimeters = millimeters; block->millimeters = hints.millimeters;
else { else {
/** /**
* Distance for interpretation of feedrate in accordance with LinuxCNC (the successor of NIST * 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 ENABLED(AUTO_POWER_CONTROL)
if (NUM_AXIS_GANG( if (NUM_AXIS_GANG(
block->steps.x, block->steps.x, || block->steps.y, || block->steps.z,
|| block->steps.y, || block->steps.i, || block->steps.j, || block->steps.k,
|| block->steps.z, || block->steps.u, || block->steps.v, || block->steps.w
|| block->steps.i,
|| block->steps.j,
|| block->steps.k,
|| block->steps.u,
|| block->steps.v,
|| block->steps.w
)) powerManager.power_on(); )) powerManager.power_on();
#endif #endif
@@ -2562,29 +2569,17 @@ bool Planner::_populate_block(
if (block->step_event_count <= acceleration_long_cutoff) { if (block->step_event_count <= acceleration_long_cutoff) {
LOGICAL_AXIS_CODE( LOGICAL_AXIS_CODE(
LIMIT_ACCEL_LONG(E_AXIS, E_INDEX_N(extruder)), LIMIT_ACCEL_LONG(E_AXIS, E_INDEX_N(extruder)),
LIMIT_ACCEL_LONG(A_AXIS, 0), LIMIT_ACCEL_LONG(A_AXIS, 0), LIMIT_ACCEL_LONG(B_AXIS, 0), LIMIT_ACCEL_LONG(C_AXIS, 0),
LIMIT_ACCEL_LONG(B_AXIS, 0), LIMIT_ACCEL_LONG(I_AXIS, 0), LIMIT_ACCEL_LONG(J_AXIS, 0), LIMIT_ACCEL_LONG(K_AXIS, 0),
LIMIT_ACCEL_LONG(C_AXIS, 0), LIMIT_ACCEL_LONG(U_AXIS, 0), LIMIT_ACCEL_LONG(V_AXIS, 0), LIMIT_ACCEL_LONG(W_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 { else {
LOGICAL_AXIS_CODE( LOGICAL_AXIS_CODE(
LIMIT_ACCEL_FLOAT(E_AXIS, E_INDEX_N(extruder)), LIMIT_ACCEL_FLOAT(E_AXIS, E_INDEX_N(extruder)),
LIMIT_ACCEL_FLOAT(A_AXIS, 0), LIMIT_ACCEL_FLOAT(A_AXIS, 0), LIMIT_ACCEL_FLOAT(B_AXIS, 0), LIMIT_ACCEL_FLOAT(C_AXIS, 0),
LIMIT_ACCEL_FLOAT(B_AXIS, 0), LIMIT_ACCEL_FLOAT(I_AXIS, 0), LIMIT_ACCEL_FLOAT(J_AXIS, 0), LIMIT_ACCEL_FLOAT(K_AXIS, 0),
LIMIT_ACCEL_FLOAT(C_AXIS, 0), LIMIT_ACCEL_FLOAT(U_AXIS, 0), LIMIT_ACCEL_FLOAT(V_AXIS, 0), LIMIT_ACCEL_FLOAT(W_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 #if HAS_DIST_MM_ARG
cart_dist_mm cart_dist_mm
#else #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 #endif
; ;
@@ -2670,7 +2668,7 @@ bool Planner::_populate_block(
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
float junction_cos_theta = LOGICAL_AXIS_GANG( float junction_cos_theta = LOGICAL_AXIS_GANG(
+ (-prev_unit_vec.e * unit_vec.e), + (-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.y * unit_vec.y),
+ (-prev_unit_vec.z * unit_vec.z), + (-prev_unit_vec.z * unit_vec.z),
+ (-prev_unit_vec.i * unit_vec.i), + (-prev_unit_vec.i * unit_vec.i),
@@ -2687,104 +2685,110 @@ bool Planner::_populate_block(
vmax_junction_sqr = sq(float(MINIMUM_PLANNER_SPEED)); vmax_junction_sqr = sq(float(MINIMUM_PLANNER_SPEED));
} }
else { else {
NOLESS(junction_cos_theta, -0.999999f); // Check for numerical round-off to avoid divide by zero.
// Convert delta vector to unit vector // Convert delta vector to unit vector
xyze_float_t junction_unit_vec = unit_vec - prev_unit_vec; xyze_float_t junction_unit_vec = unit_vec - prev_unit_vec;
normalize_junction_vector(junction_unit_vec); normalize_junction_vector(junction_unit_vec);
const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec), const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec);
sin_theta_d2 = SQRT(0.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive.
vmax_junction_sqr = junction_acceleration * junction_deviation_mm * sin_theta_d2 / (1.0f - sin_theta_d2); if (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 vmax_junction_sqr = junction_acceleration * junction_deviation_mm * sin_theta_d2 / (1.0f - sin_theta_d2);
if (block->millimeters < 1 && junction_cos_theta < -0.7071067812f) {
#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) #error "TODO: Inline maths with the MCU / FPU."
// Based on LUT table and linear interpolation
/** #elif ENABLED(JD_USE_LOOKUP_TABLE)
* // Generate the JD Lookup Table
* constexpr float c = 1.00751495f; // Correction factor to center error around 0
* for (int i = 0; i < jd_lut_count - 1; ++i) {
* const float x0 = (sq(i) - 1) / sq(i),
* y0 = acos(x0) * (i == 0 ? 1 : c),
* x1 = i < jd_lut_count - 1 ? 0.5 * x0 + 0.5 : 0.999999f,
* y1 = acos(x1) * (i < jd_lut_count - 1 ? c : 1);
* jd_lut_k[i] = (y0 - y1) / (x0 - x1);
* jd_lut_b[i] = (y1 * x0 - y0 * x1) / (x0 - x1);
* }
*
* // Compute correction factor (Set c to 1.0f first!)
* float min = INFINITY, max = -min;
* for (float t = 0; t <= 1; t += 0.0003f) {
* const float e = acos(t) / approx(t);
* if (isfinite(e)) {
* if (e < min) min = e;
* if (e > max) max = e;
* }
* }
* fprintf(stderr, "%.9gf, ", (min + max) / 2);
*/
static constexpr int16_t jd_lut_count = 16;
static constexpr uint16_t jd_lut_tll = _BV(jd_lut_count - 1);
static constexpr int16_t jd_lut_tll0 = __builtin_clz(jd_lut_tll) + 1; // i.e., 16 - jd_lut_count + 1
static constexpr float jd_lut_k[jd_lut_count] PROGMEM = {
-1.03145837f, -1.30760646f, -1.75205851f, -2.41705704f,
-3.37769222f, -4.74888992f, -6.69649887f, -9.45661736f,
-13.3640480f, -18.8928222f, -26.7136841f, -37.7754593f,
-53.4201813f, -75.5458374f, -106.836761f, -218.532821f };
static constexpr float jd_lut_b[jd_lut_count] PROGMEM = {
1.57079637f, 1.70887053f, 2.04220939f, 2.62408352f,
3.52467871f, 4.85302639f, 6.77020454f, 9.50875854f,
13.4009285f, 18.9188995f, 26.7321243f, 37.7885055f,
53.4293975f, 75.5523529f, 106.841369f, 218.534011f };
const float neg = junction_cos_theta < 0 ? -1 : 1, // Fast acos approximation (max. error +-0.01 rads)
t = neg * junction_cos_theta; // 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]); const float neg = junction_cos_theta < 0 ? -1 : 1,
if (neg > 0) junction_theta = RADIANS(180) - junction_theta; // acos(-t) 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°) float junction_theta = t * pgm_read_float(&jd_lut_k[idx]) + pgm_read_float(&jd_lut_b[idx]);
// Based on MinMax polynomial published by W. Randolph Franklin, see if (neg > 0) junction_theta = RADIANS(180) - junction_theta; // acos(-t)
// https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
// acos( t) = pi / 2 - asin(x)
// acos(-t) = pi - acos(t) ... pi / 2 + asin(x)
const float neg = junction_cos_theta < 0 ? -1 : 1, #else
t = neg * junction_cos_theta,
asinx = 0.032843707f
+ t * (-1.451838349f
+ t * ( 29.66153956f
+ t * (-131.1123477f
+ t * ( 262.8130562f
+ t * (-242.7199627f
+ t * ( 84.31466202f ) ))))),
junction_theta = RADIANS(90) + neg * asinx; // acos(-t)
// NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0. // 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; // NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0.
NOMORE(vmax_junction_sqr, limit_sqr);
}
#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 // Get the lowest speed
@@ -2944,12 +2948,11 @@ bool Planner::_populate_block(
} // _populate_block() } // _populate_block()
/** /**
* Planner::buffer_sync_block * @brief Add a block to the buffer that just updates the position
* 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 BLOCK_FLAG_SYNC_FANS & BLOCK_FLAG_LASER_PWR *
* 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*/) { void Planner::buffer_sync_block(const BlockFlagBit sync_flag/*=BLOCK_BIT_SYNC_POSITION*/) {
// Wait for the next available block // 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); block_t * const block = get_next_free_block(next_buffer_head);
// Clear block // Clear block
memset(block, 0, sizeof(block_t)); block->reset();
block->flag.apply(sync_flag); block->flag.apply(sync_flag);
block->position = position; block->position = position;
#if ENABLED(BACKLASH_COMPENSATION) #if ENABLED(BACKLASH_COMPENSATION)
LOOP_NUM_AXES(axis) block->position[axis] += backlash.get_applied_steps((AxisEnum)axis); LOOP_NUM_AXES(axis) block->position[axis] += backlash.get_applied_steps((AxisEnum)axis);
#endif #endif
#if BOTH(HAS_FAN, LASER_SYNCHRONOUS_M106_M107) #if BOTH(HAS_FAN, LASER_SYNCHRONOUS_M106_M107)
FANS_LOOP(i) block->fan_speed[i] = thermalManager.fan_speed[i]; FANS_LOOP(i) block->fan_speed[i] = thermalManager.fan_speed[i];
#endif #endif
@@ -2991,22 +2993,24 @@ void Planner::buffer_sync_block(const BlockFlagBit sync_flag/*=BLOCK_BIT_SYNC_PO
} // buffer_sync_block() } // 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 * @return false if no segment was queued due to cleaning, cold extrusion, full queue, etc.
* 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.
*/ */
bool Planner::buffer_segment(const abce_pos_t &abce bool Planner::buffer_segment(const abce_pos_t &abce
OPTARG(HAS_DIST_MM_ARG, const xyze_float_t &cart_dist_mm) 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 // 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 if (!_buffer_steps(target
OPTARG(HAS_POSITION_FLOAT, target_float) OPTARG(HAS_POSITION_FLOAT, target_float)
OPTARG(HAS_DIST_MM_ARG, cart_dist_mm) OPTARG(HAS_DIST_MM_ARG, cart_dist_mm)
, fr_mm_s, extruder, millimeters) , fr_mm_s, extruder, hints
) return false; )) return false;
stepper.wake_up(); stepper.wake_up();
return true; return true;
@@ -3126,12 +3130,12 @@ bool Planner::buffer_segment(const abce_pos_t &abce
* *
* cart - target position in mm or degrees * cart - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s) * fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder * extruder - optional target extruder (otherwise active_extruder)
* millimeters - the length of the movement, if known * hints - optional parameters to aid planner calculations
* inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled)
*/ */
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*/ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s
OPTARG(SCARA_FEEDRATE_SCALING, const_float_t inv_duration/*=0.0*/) , const uint8_t extruder/*=active_extruder*/
, const PlannerHints &hints/*=PlannerHints()*/
) { ) {
xyze_pos_t machine = cart; xyze_pos_t machine = cart;
TERN_(HAS_POSITION_MODIFIERS, apply_modifiers(machine)); 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 #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' // Cartesian XYZ to kinematic ABC, stored in global 'delta'
inverse_kinematics(machine); 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) #if ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feedrate from mm/s to degrees/s // For SCARA scale the feedrate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time. // 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 xyz_pos_t diff = delta - position_float;
const feedRate_t feedrate = diff.magnitude() * duration_recip; const feedRate_t feedrate = diff.magnitude() * duration_recip;
#else #else
const feedRate_t feedrate = fr_mm_s; const feedRate_t feedrate = fr_mm_s;
#endif #endif
TERN_(HAS_EXTRUDERS, delta.e = machine.e); 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; position_cart = cart;
return true; return true;
} }
return false; return false;
#else #else
return buffer_segment(machine, fr_mm_s, extruder, millimeters); return buffer_segment(machine, fr_mm_s, extruder, hints);
#endif #endif
} // buffer_line() } // buffer_line()

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

@@ -280,6 +280,8 @@ typedef struct block_t {
block_laser_t laser; block_laser_t laser;
#endif #endif
void reset() { memset((char*)this, 0, sizeof(*this)); }
} block_t; } block_t;
#if ANY(LIN_ADVANCE, SCARA_FEEDRATE_SCALING, GRADIENT_MIX, LCD_SHOW_E_TOTAL, POWER_LOSS_RECOVERY) #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; typedef IF<(BLOCK_BUFFER_SIZE > 64), uint16_t, uint8_t>::type last_move_t;
#endif #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 { class Planner {
public: public:
@@ -752,14 +778,14 @@ class Planner {
* target - target position in steps units * target - target position in steps units
* fr_mm_s - (target) speed of the move * fr_mm_s - (target) speed of the move
* extruder - target extruder * 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 * Returns true if movement was buffered, false otherwise
*/ */
static bool _buffer_steps(const xyze_long_t &target static bool _buffer_steps(const xyze_long_t &target
OPTARG(HAS_POSITION_FLOAT, const xyze_pos_t &target_float) OPTARG(HAS_POSITION_FLOAT, const xyze_pos_t &target_float)
OPTARG(HAS_DIST_MM_ARG, const xyze_float_t &cart_dist_mm) 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 cart_dist_mm The pre-calculated move lengths for all axes, in mm
* @param fr_mm_s (target) speed of the move * @param fr_mm_s (target) speed of the move
* @param extruder target extruder * @param extruder target extruder
* @param millimeters A pre-calculated linear distance for the move, in mm, * @param hints parameters to aid planner calculations
* or 0.0 to have the distance calculated here.
* *
* @return true if movement is acceptable, false otherwise * @return true if movement is acceptable, false otherwise
*/ */
static bool _populate_block(block_t * const block, const xyze_long_t &target 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_POSITION_FLOAT, const xyze_pos_t &target_float)
OPTARG(HAS_DIST_MM_ARG, const xyze_float_t &cart_dist_mm) 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 * a,b,c,e - target positions in mm and/or degrees
* fr_mm_s - (target) speed of the move * fr_mm_s - (target) speed of the move
* extruder - target extruder * extruder - optional target extruder (otherwise active_extruder)
* millimeters - the length of the movement, if known * hints - optional parameters to aid planner calculations
*/ */
static bool buffer_segment(const abce_pos_t &abce static bool buffer_segment(const abce_pos_t &abce
OPTARG(HAS_DIST_MM_ARG, const xyze_float_t &cart_dist_mm) 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: public:
@@ -826,12 +853,12 @@ class Planner {
* *
* cart - target position in mm or degrees * cart - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s) * fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder * extruder - optional target extruder (otherwise active_extruder)
* millimeters - the length of the movement, if known * hints - optional parameters to aid planner calculations
* inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled)
*/ */
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 static bool buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s
OPTARG(SCARA_FEEDRATE_SCALING, const_float_t inv_duration=0.0) , const uint8_t extruder=active_extruder
, const PlannerHints &hints=PlannerHints()
); );
#if ENABLED(DIRECT_STEPPING) #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 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 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 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 #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; millis_t next_idle_ms = millis() + 200UL;
// Hints to help optimize the move
PlannerHints hints;
for (float t = 0; t < 1;) { for (float t = 0; t < 1;) {
thermalManager.task(); thermalManager.task();
@@ -177,7 +180,7 @@ void cubic_b_spline(
} }
*/ */
step = new_t - t; hints.millimeters = new_t - t;
t = new_t; t = new_t;
// Compute and send new position // Compute and send new position
@@ -203,7 +206,7 @@ void cubic_b_spline(
const xyze_pos_t &pos = bez_target; const xyze_pos_t &pos = bez_target;
#endif #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; 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; else if (LA_steps) nextAdvanceISR = 0;
#endif #endif
/* /**
* Adjust Laser Power - Accelerating * 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. * isPowered - True when a move is powered.
* trap_ramp_entry_incr - holds the precalculated value to increase the current power per accel step. * 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. * 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; uint32_t step_rate;
#if ENABLED(S_CURVE_ACCELERATION) #if ENABLED(S_CURVE_ACCELERATION)
// If this is the 1st time we process the 2nd half of the trapezoid... // If this is the 1st time we process the 2nd half of the trapezoid...
if (!bezier_2nd_half) { if (!bezier_2nd_half) {
// Initialize the Bézier speed curve // Initialize the Bézier speed curve
@@ -2046,6 +2048,7 @@ uint32_t Stepper::block_phase_isr() {
? _eval_bezier_curve(deceleration_time) ? _eval_bezier_curve(deceleration_time)
: current_block->final_rate; : current_block->final_rate;
} }
#else #else
// Using the old trapezoidal control // Using the old trapezoidal control
step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate); step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate);
@@ -2055,9 +2058,8 @@ uint32_t Stepper::block_phase_isr() {
} }
else else
step_rate = current_block->final_rate; step_rate = current_block->final_rate;
#endif
// step_rate is in steps/second #endif
// step_rate to timer interval and steps per stepper isr // step_rate to timer interval and steps per stepper isr
interval = calc_timer_interval(step_rate, &steps_per_isr); interval = calc_timer_interval(step_rate, &steps_per_isr);
@@ -2109,10 +2111,10 @@ uint32_t Stepper::block_phase_isr() {
interval = ticks_nominal; interval = ticks_nominal;
} }
/* Adjust Laser Power - Cruise /**
* Adjust Laser Power - Cruise
* power - direct or floor adjusted active laser power. * power - direct or floor adjusted active laser power.
*/ */
#if ENABLED(LASER_POWER_TRAP) #if ENABLED(LASER_POWER_TRAP)
if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) {
if (step_events_completed + 1 == accelerate_until) { if (step_events_completed + 1 == accelerate_until) {
@@ -2130,7 +2132,7 @@ uint32_t Stepper::block_phase_isr() {
} }
#if ENABLED(LASER_FEATURE) #if ENABLED(LASER_FEATURE)
/* /**
* CUTTER_MODE_DYNAMIC is experimental and developing. * 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. * 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. * 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 else { // !current_block
#if ENABLED(LASER_FEATURE) #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 cutter.apply_power(0); // No movement in dynamic mode so turn Laser off
}
#endif #endif
} }