From 4d4bf7897d1f99ce26062126d3ec9982642b7a3a Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Tue, 8 May 2018 03:13:43 -0500 Subject: [PATCH] Junction Deviation jerk limiting option --- .travis.yml | 2 +- Marlin/Configuration_adv.h | 9 + Marlin/src/config/default/Configuration_adv.h | 9 + Marlin/src/module/planner.cpp | 235 ++++++++++-------- 4 files changed, 152 insertions(+), 103 deletions(-) diff --git a/.travis.yml b/.travis.yml index 613155fab..4f128613a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -84,7 +84,7 @@ script: - opt_set TEMP_SENSOR_4 999 - opt_set TEMP_SENSOR_BED 1 - opt_enable AUTO_BED_LEVELING_UBL RESTORE_LEVELING_AFTER_G28 DEBUG_LEVELING_FEATURE G26_MESH_EDITING ENABLE_LEVELING_FADE_HEIGHT EEPROM_SETTINGS EEPROM_CHITCHAT G3D_PANEL SKEW_CORRECTION - - opt_enable_adv CUSTOM_USER_MENUS I2C_POSITION_ENCODERS BABYSTEPPING BABYSTEP_XY LIN_ADVANCE NANODLP_Z_SYNC QUICK_HOME + - opt_enable_adv CUSTOM_USER_MENUS I2C_POSITION_ENCODERS BABYSTEPPING BABYSTEP_XY LIN_ADVANCE NANODLP_Z_SYNC QUICK_HOME JUNCTION_DEVIATION - build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM} # # Add a Sled Z Probe, use UBL Cartesian moves, use Japanese language diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index caad96a40..68ed4eb95 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -431,6 +431,15 @@ // if unwanted behavior is observed on a user's machine when running at very slow speeds. #define MINIMUM_PLANNER_SPEED 0.05 // (mm/sec) +// +// Use Junction Deviation instead of traditional Jerk Limiting +// +//#define JUNCTION_DEVIATION +#if ENABLED(JUNCTION_DEVIATION) + #define JUNCTION_DEVIATION_FACTOR 0.02 + //#define JUNCTION_DEVIATION_INCLUDE_E +#endif + // Microstep setting (Only functional when stepper driver microstep pins are connected to MCU. #define MICROSTEP_MODES {16,16,16,16,16} // [1,2,4,8,16] diff --git a/Marlin/src/config/default/Configuration_adv.h b/Marlin/src/config/default/Configuration_adv.h index caad96a40..68ed4eb95 100644 --- a/Marlin/src/config/default/Configuration_adv.h +++ b/Marlin/src/config/default/Configuration_adv.h @@ -431,6 +431,15 @@ // if unwanted behavior is observed on a user's machine when running at very slow speeds. #define MINIMUM_PLANNER_SPEED 0.05 // (mm/sec) +// +// Use Junction Deviation instead of traditional Jerk Limiting +// +//#define JUNCTION_DEVIATION +#if ENABLED(JUNCTION_DEVIATION) + #define JUNCTION_DEVIATION_FACTOR 0.02 + //#define JUNCTION_DEVIATION_INCLUDE_E +#endif + // Microstep setting (Only functional when stepper driver microstep pins are connected to MCU. #define MICROSTEP_MODES {16,16,16,16,16} // [1,2,4,8,16] diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 7cbbbfaf7..109e74552 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -787,8 +787,8 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e #if ENABLED(BEZIER_JERK_CONTROL) // Jerk controlled speed requires to express speed versus time, NOT steps - uint32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * HAL_STEPPER_TIMER_RATE, - deceleration_time = ((float)(cruise_rate - final_rate) / accel) * HAL_STEPPER_TIMER_RATE; + uint32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * (HAL_STEPPER_TIMER_RATE), + deceleration_time = ((float)(cruise_rate - final_rate) / accel) * (HAL_STEPPER_TIMER_RATE); // And to offload calculations from the ISR, we also calculate the inverse of those times here uint32_t acceleration_time_inverse = get_period_inverse(acceleration_time); @@ -1864,129 +1864,161 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE] } #endif - // Initial limit on the segment entry velocity - float vmax_junction; - - #if 0 // Use old jerk for now + float vmax_junction; // Initial limit on the segment entry velocity + + #if ENABLED(JUNCTION_DEVIATION) + + /** + * Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + * Let a circle be tangent to both previous and current path line segments, where the junction + * deviation is defined as the distance from the junction to the closest edge of the circle, + * colinear with the circle center. The circular segment joining the two paths represents the + * path of centripetal acceleration. Solve for max velocity based on max acceleration about the + * radius of the circle, defined indirectly by junction deviation. This may be also viewed as + * path width or max_jerk in the previous Grbl version. This approach does not actually deviate + * from path, but used as a robust way to compute cornering speeds, as it takes into account the + * nonlinearities of both the junction angle and junction velocity. + * + * NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + * mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + * stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + * is exactly the same. Instead of motioning all the way to junction point, the machine will + * just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + * a continuous mode path, but ARM-based microcontrollers most certainly do. + * + * NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be + * changed dynamically during operation nor can the line move geometry. This must be kept in + * memory in the event of a feedrate override changing the nominal speeds of blocks, which can + * change the overall maximum entry speed conditions of all blocks. + */ - float junction_deviation = 0.1; + // Unit vector of previous path line segment + static float previous_unit_vec[ + #if ENABLED(JUNCTION_DEVIATION_INCLUDE_E) + XYZE + #else + XYZ + #endif + ]; - // Compute path unit vector - double unit_vec[XYZ] = { + float unit_vec[] = { delta_mm[A_AXIS] * inverse_millimeters, delta_mm[B_AXIS] * inverse_millimeters, delta_mm[C_AXIS] * inverse_millimeters + #if ENABLED(JUNCTION_DEVIATION_INCLUDE_E) + , delta_mm[E_AXIS] * inverse_millimeters + #endif }; - /* - Compute maximum allowable entry speed at junction by centripetal acceleration approximation. - - Let a circle be tangent to both previous and current path line segments, where the junction - deviation is defined as the distance from the junction to the closest edge of the circle, - collinear with the circle center. - - The circular segment joining the two paths represents the path of centripetal acceleration. - Solve for max velocity based on max acceleration about the radius of the circle, defined - indirectly by junction deviation. - - This may be also viewed as path width or max_jerk in the previous grbl version. This approach - does not actually deviate from path, but used as a robust way to compute cornering speeds, as - it takes into account the nonlinearities of both the junction angle and junction velocity. - */ - - vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed - // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) { // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. - const float cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS]; - // Skip and use default max junction speed for 0 degree acute junction. - if (cos_theta < 0.95) { - vmax_junction = min(previous_nominal_speed, block->nominal_speed); - // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. - if (cos_theta > -0.95) { - // Compute maximum junction velocity based on maximum acceleration and junction deviation - float sin_theta_d2 = SQRT(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive. - NOMORE(vmax_junction, SQRT(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2))); - } + float junction_cos_theta = -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + -previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] + -previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] + #if ENABLED(JUNCTION_DEVIATION_INCLUDE_E) + -previous_unit_vec[E_AXIS] * unit_vec[E_AXIS] + #endif + ; + + // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). + if (junction_cos_theta > 0.999999) { + // For a 0 degree acute junction, just set minimum junction speed. + vmax_junction = MINIMUM_PLANNER_SPEED; + } + else { + junction_cos_theta = max(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero. + const float sin_theta_d2 = SQRT(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive. + + // TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the + // two junctions. However, this shouldn't be a significant problem except in extreme circumstances. + vmax_junction = SQRT((block->acceleration * JUNCTION_DEVIATION_FACTOR * sin_theta_d2) / (1.0 - sin_theta_d2)); } + + vmax_junction = MIN3(vmax_junction, block->nominal_speed, previous_nominal_speed); } - #endif + else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later. + vmax_junction = 0.0; - /** - * Adapted from Průša MKS firmware - * https://github.com/prusa3d/Prusa-Firmware - * - * Start with a safe speed (from which the machine may halt to stop immediately). - */ + COPY(previous_unit_vec, unit_vec); - // Exit speed limited by a jerk to full halt of a previous last segment - static float previous_safe_speed; + #else // Classic Jerk Limiting - float safe_speed = block->nominal_speed; - uint8_t limited = 0; - LOOP_XYZE(i) { - const float jerk = FABS(current_speed[i]), maxj = max_jerk[i]; - if (jerk > maxj) { - if (limited) { - const float mjerk = maxj * block->nominal_speed; - if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk; - } - else { - ++limited; - safe_speed = maxj; + /** + * Adapted from Průša MKS firmware + * https://github.com/prusa3d/Prusa-Firmware + * + * Start with a safe speed (from which the machine may halt to stop immediately). + */ + + // Exit speed limited by a jerk to full halt of a previous last segment + static float previous_safe_speed; + + float safe_speed = block->nominal_speed; + uint8_t limited = 0; + LOOP_XYZE(i) { + const float jerk = FABS(current_speed[i]), maxj = max_jerk[i]; + if (jerk > maxj) { + if (limited) { + const float mjerk = maxj * block->nominal_speed; + if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk; + } + else { + ++limited; + safe_speed = maxj; + } } } - } - if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) { - // Estimate a maximum velocity allowed at a joint of two successive segments. - // If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities, - // then the machine is not coasting anymore and the safe entry / exit velocities shall be used. - - // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum. - // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting. - vmax_junction = min(block->nominal_speed, previous_nominal_speed); - - // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities. - float v_factor = 1; - limited = 0; - - // Now limit the jerk in all axes. - const float smaller_speed_factor = vmax_junction / previous_nominal_speed; - LOOP_XYZE(axis) { - // Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop. - float v_exit = previous_speed[axis] * smaller_speed_factor, - v_entry = current_speed[axis]; - if (limited) { - v_exit *= v_factor; - v_entry *= v_factor; - } + if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) { + // Estimate a maximum velocity allowed at a joint of two successive segments. + // If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities, + // then the machine is not coasting anymore and the safe entry / exit velocities shall be used. + + // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum. + // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting. + vmax_junction = min(block->nominal_speed, previous_nominal_speed); + + // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities. + float v_factor = 1; + limited = 0; + + // Now limit the jerk in all axes. + const float smaller_speed_factor = vmax_junction / previous_nominal_speed; + LOOP_XYZE(axis) { + // Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop. + float v_exit = previous_speed[axis] * smaller_speed_factor, + v_entry = current_speed[axis]; + if (limited) { + v_exit *= v_factor; + v_entry *= v_factor; + } - // Calculate jerk depending on whether the axis is coasting in the same direction or reversing. - const float jerk = (v_exit > v_entry) - ? // coasting axis reversal - ( (v_entry > 0 || v_exit < 0) ? (v_exit - v_entry) : max(v_exit, -v_entry) ) - : // v_exit <= v_entry coasting axis reversal - ( (v_entry < 0 || v_exit > 0) ? (v_entry - v_exit) : max(-v_exit, v_entry) ); + // Calculate jerk depending on whether the axis is coasting in the same direction or reversing. + const float jerk = (v_exit > v_entry) + ? // coasting axis reversal + ( (v_entry > 0 || v_exit < 0) ? (v_exit - v_entry) : max(v_exit, -v_entry) ) + : // v_exit <= v_entry coasting axis reversal + ( (v_entry < 0 || v_exit > 0) ? (v_entry - v_exit) : max(-v_exit, v_entry) ); - if (jerk > max_jerk[axis]) { - v_factor *= max_jerk[axis] / jerk; - ++limited; + if (jerk > max_jerk[axis]) { + v_factor *= max_jerk[axis] / jerk; + ++limited; + } } + if (limited) vmax_junction *= v_factor; + // Now the transition velocity is known, which maximizes the shared exit / entry velocity while + // respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints. + const float vmax_junction_threshold = vmax_junction * 0.99f; + if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold) + vmax_junction = safe_speed; } - if (limited) vmax_junction *= v_factor; - // Now the transition velocity is known, which maximizes the shared exit / entry velocity while - // respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints. - const float vmax_junction_threshold = vmax_junction * 0.99f; - if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold) + else vmax_junction = safe_speed; - } - else - vmax_junction = safe_speed; + + previous_safe_speed = safe_speed; + #endif // Classic Jerk Limiting // Max entry speed of this block equals the max exit speed of the previous block. block->max_entry_speed = vmax_junction; @@ -2010,7 +2042,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE] // Update previous path unit_vector and nominal speed COPY(previous_speed, current_speed); previous_nominal_speed = block->nominal_speed; - previous_safe_speed = safe_speed; // Move buffer head block_buffer_head = next_buffer_head;