From b826bf41945b453067c3516eafe67b8ed035f0ed Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Mon, 30 Jul 2018 22:50:08 -0500 Subject: [PATCH] Fix initial safe_speed in jerk code (#11417) --- Marlin/src/module/planner.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 22fda4ab98..c93d811271 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -1348,7 +1348,7 @@ void Planner::check_axes_activity() { volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = parser.volumetric_enabled ? ratio_2 / CIRCLE_AREA(filament_width_nominal * 0.5f) // Volumetric uses a true volumetric multiplier - : ratio_2; // Linear squares the ratio, which scales the volume + : ratio_2; // Linear squares the ratio, which scales the volume refresh_e_factor(FILAMENT_SENSOR_EXTRUDER_NUM); } @@ -1947,7 +1947,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move, else block->millimeters = millimeters; - const float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides + const float inverse_millimeters = 1.0f / block->millimeters; // Inverse millimeters to remove multiple divides // Calculate inverse time for this move. No divide by zero due to previous checks. // Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0. @@ -2298,27 +2298,27 @@ bool Planner::_populate_block(block_t * const block, bool split_move, /** * 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). */ + const float nominal_speed = SQRT(block->nominal_speed_sqr); // Exit speed limited by a jerk to full halt of a previous last segment static float previous_safe_speed; - const float nominal_speed = SQRT(block->nominal_speed_sqr); + // Start with a safe speed (from which the machine may halt to stop immediately). float safe_speed = nominal_speed; uint8_t limited = 0; LOOP_XYZE(i) { - const float jerk = ABS(current_speed[i]), maxj = max_jerk[i]; - if (jerk > maxj) { - if (limited) { - const float mjerk = maxj * nominal_speed; - if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk; + const float jerk = ABS(current_speed[i]), // cs : Starting from zero, change in speed for this axis + maxj = max_jerk[i]; // mj : The max jerk setting for this axis + if (jerk > maxj) { // cs > mj : New current speed too fast? + if (limited) { // limited already? + const float mjerk = nominal_speed * maxj; // ns*mj + if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk; // ns*mj/cs } else { - ++limited; - safe_speed = maxj; + safe_speed *= maxj / jerk; // Initial limit: ns*mj/cs + ++limited; // Initially limited } } } @@ -2620,7 +2620,7 @@ void Planner::reset_acceleration_rates() { // Recalculate position, steps_to_mm if axis_steps_per_mm changes! void Planner::refresh_positioning() { - LOOP_XYZE_N(i) steps_to_mm[i] = 1.0 / axis_steps_per_mm[i]; + LOOP_XYZE_N(i) steps_to_mm[i] = 1.0f / axis_steps_per_mm[i]; set_position_mm_kinematic(current_position); reset_acceleration_rates(); }