From 0c78a6f65791e1c2c3f4278ed2b48bcf20b9b063 Mon Sep 17 00:00:00 2001 From: tombrazier <68918209+tombrazier@users.noreply.github.com> Date: Fri, 8 Jul 2022 20:41:39 +0100 Subject: [PATCH] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20Optimize=20G2-G3=20Arcs=20?= =?UTF-8?q?(#24366)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/feature/bedlevel/ubl/ubl_motion.cpp | 17 +- Marlin/src/feature/joystick.cpp | 3 +- Marlin/src/gcode/motion/G2_G3.cpp | 26 +- Marlin/src/module/motion.cpp | 32 +- Marlin/src/module/planner.cpp | 392 +++++++++--------- Marlin/src/module/planner.h | 61 ++- Marlin/src/module/planner_bezier.cpp | 7 +- Marlin/src/module/stepper.cpp | 27 +- 8 files changed, 310 insertions(+), 255 deletions(-) diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp index 9fa2257dc8..8121a0b9b5 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp +++ b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp @@ -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 diff --git a/Marlin/src/feature/joystick.cpp b/Marlin/src/feature/joystick.cpp index daa642d32e..acab5d7437 100644 --- a/Marlin/src/feature/joystick.cpp +++ b/Marlin/src/feature/joystick.cpp @@ -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; } } diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp index 14ef9ac2a6..c45204c6f6 100644 --- a/Marlin/src/gcode/motion/G2_G3.cpp +++ b/Marlin/src/gcode/motion/G2_G3.cpp @@ -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( diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index b3b607e677..eaf3ab5409 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -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 diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index bc5bfd3dfc..7b00552dff 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -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() diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index e0aa89ab72..5a0de47bf2 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -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 diff --git a/Marlin/src/module/planner_bezier.cpp b/Marlin/src/module/planner_bezier.cpp index 93b118f330..a3f98435d0 100644 --- a/Marlin/src/module/planner_bezier.cpp +++ b/Marlin/src/module/planner_bezier.cpp @@ -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; } } diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 4832220abd..2bacc55606 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -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 }