From d00b4a2c75ff16183de65031f1458b4bc87f07ff Mon Sep 17 00:00:00 2001 From: bubnikv Date: Fri, 22 Jul 2016 16:52:13 +0200 Subject: [PATCH] Reworked the calculation of jerks in the planner. Now the confugration values are half the values used before, and the planner ensures, that the jerks will not be violated. --- Firmware/Configuration.h | 7 +- Firmware/ConfigurationStore.cpp | 14 +-- Firmware/Marlin_main.cpp | 9 +- Firmware/planner.cpp | 182 ++++++++++++++------------------ Firmware/planner.h | 10 +- 5 files changed, 102 insertions(+), 120 deletions(-) diff --git a/Firmware/Configuration.h b/Firmware/Configuration.h index 1684304e..e3e4e16d 100644 --- a/Firmware/Configuration.h +++ b/Firmware/Configuration.h @@ -405,9 +405,10 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of // #define EXTRUDER_OFFSET_Y {0.0, 5.00} // (in mm) for each extruder, offset of the hotend on the Y axis // The speed change that does not require acceleration (i.e. the software might assume it can be done instantaneously) -#define DEFAULT_XYJERK 20.0 // (mm/sec) -#define DEFAULT_ZJERK 0.4 // (mm/sec) -#define DEFAULT_EJERK 5.0 // (mm/sec) +#define DEFAULT_XJERK 10.0 // (mm/sec) +#define DEFAULT_YJERK 10.0 // (mm/sec) +#define DEFAULT_ZJERK 0.2 // (mm/sec) +#define DEFAULT_EJERK 2.5 // (mm/sec) //=========================================================================== //=============================Additional Features=========================== diff --git a/Firmware/ConfigurationStore.cpp b/Firmware/ConfigurationStore.cpp index 097d6c0d..b9ef874f 100644 --- a/Firmware/ConfigurationStore.cpp +++ b/Firmware/ConfigurationStore.cpp @@ -169,9 +169,10 @@ void Config_PrintSettings() SERIAL_ECHOPAIR(" M205 S",minimumfeedrate ); SERIAL_ECHOPAIR(" T" ,mintravelfeedrate ); SERIAL_ECHOPAIR(" B" ,minsegmenttime ); - SERIAL_ECHOPAIR(" X" ,max_xy_jerk ); - SERIAL_ECHOPAIR(" Z" ,max_z_jerk); - SERIAL_ECHOPAIR(" E" ,max_e_jerk); + SERIAL_ECHOPAIR(" X" ,max_jerk[X_AXIS] ); + SERIAL_ECHOPAIR(" Y" ,max_jerk[Y_AXIS] ); + SERIAL_ECHOPAIR(" Z" ,max_jerk[Z_AXIS] ); + SERIAL_ECHOPAIR(" E" ,max_jerk[E_AXIS] ); SERIAL_ECHOLN(""); SERIAL_ECHO_START; @@ -356,9 +357,10 @@ void Config_ResetDefault() minimumfeedrate=DEFAULT_MINIMUMFEEDRATE; minsegmenttime=DEFAULT_MINSEGMENTTIME; mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE; - max_xy_jerk=DEFAULT_XYJERK; - max_z_jerk=DEFAULT_ZJERK; - max_e_jerk=DEFAULT_EJERK; + max_jerk[X_AXIS] = DEFAULT_XJERK; + max_jerk[Y_AXIS] = DEFAULT_YJERK; + max_jerk[Z_AXIS] = DEFAULT_ZJERK; + max_jerk[E_AXIS] = DEFAULT_EJERK; add_homing[X_AXIS] = add_homing[Y_AXIS] = add_homing[Z_AXIS] = 0; #ifdef ULTIPANEL plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP; diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index 90e7ebda..5f45ad60 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -3515,7 +3515,7 @@ Sigma_Exit: float value = code_value(); if(value < 20.0) { float factor = axis_steps_per_unit[i] / value; // increase e constants if M92 E14 is given for netfab. - max_e_jerk *= factor; + max_jerk[E_AXIS] *= factor; max_feedrate[i] *= factor; axis_steps_per_sqr_second[i] *= factor; } @@ -3718,9 +3718,10 @@ Sigma_Exit: if(code_seen('S')) minimumfeedrate = code_value(); if(code_seen('T')) mintravelfeedrate = code_value(); if(code_seen('B')) minsegmenttime = code_value() ; - if(code_seen('X')) max_xy_jerk = code_value() ; - if(code_seen('Z')) max_z_jerk = code_value() ; - if(code_seen('E')) max_e_jerk = code_value() ; + if(code_seen('X')) max_jerk[X_AXIS] = max_jerk[Y_AXIS] = code_value(); + if(code_seen('Y')) max_jerk[Y_AXIS] = code_value(); + if(code_seen('Z')) max_jerk[Z_AXIS] = code_value(); + if(code_seen('E')) max_jerk[E_AXIS] = code_value(); } break; case 206: // M206 additional homing offset diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp index 39c4aca7..3d574017 100644 --- a/Firmware/planner.cpp +++ b/Firmware/planner.cpp @@ -74,9 +74,8 @@ unsigned long max_acceleration_units_per_sq_second[NUM_AXIS]; // Use M201 to ove float minimumfeedrate; float acceleration; // Normal acceleration mm/s^2 THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX float retract_acceleration; // mm/s^2 filament pull-pack and push-forward while standing still in the other axis M204 TXXXX -float max_xy_jerk; //speed than can be stopped at once, if i understand correctly. -float max_z_jerk; -float max_e_jerk; +// Jerk is a maximum immediate velocity change. +float max_jerk[NUM_AXIS]; float mintravelfeedrate; unsigned long axis_steps_per_sqr_second[NUM_AXIS]; @@ -93,6 +92,7 @@ matrix_3x3 plan_bed_level_matrix = { long position[NUM_AXIS]; //rescaled from extern when axis_steps_per_unit are changed by gcode static float previous_speed[NUM_AXIS]; // Speed of previous path line segment static float previous_nominal_speed; // Nominal speed of previous path line segment +static float previous_safe_speed; // Exit speed limited by a jerk to full halt of a previous last segment. #ifdef AUTOTEMP float autotemp_max=250; @@ -279,7 +279,7 @@ void planner_recalculate(const float &safe_final_speed) tail = block_index; // Update the number of blocks to process. n_blocks = (block_buffer_head + BLOCK_BUFFER_SIZE - tail) & (BLOCK_BUFFER_SIZE - 1); - SERIAL_ECHOLNPGM("BLOCK_FLAG_START_FROM_FULL_HALT"); + // SERIAL_ECHOLNPGM("START"); break; } // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. @@ -878,44 +878,40 @@ Having the real displacement of the head, we can calculate the total movement le } #endif // Start with a safe speed. - //Vojtech: This code tries to limit the initial jerk to half of the maximum jerk value. - //The code is not quite correct. It is pessimistic as it shall limit a projection of the jerk into each axis, - //but when the current code clamps, it clamps as if the movement is done in a single axis only. - float vmax_junction = max_xy_jerk/2.f; - if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2.f) - vmax_junction = min(vmax_junction, max_z_jerk/2.f); - if(fabs(current_speed[E_AXIS]) > max_e_jerk/2.f) - vmax_junction = min(vmax_junction, max_e_jerk/2.f); - vmax_junction = min(vmax_junction, block->nominal_speed); // Safe speed is the speed, from which the machine may halt to stop immediately. - float safe_speed = vmax_junction; + float safe_speed = block->nominal_speed; + bool limited = false; + for (uint8_t axis = 0; axis < 4; ++ axis) { + float jerk = fabs(current_speed[axis]); + if (jerk > max_jerk[axis]) { + // The actual jerk is lower, if it has been limited by the XY jerk. + if (limited) { + // Spare one division by a following gymnastics: + // Instead of jerk *= safe_speed / block->nominal_speed, + // multiply max_jerk[axis] by the divisor. + jerk *= safe_speed; + float mjerk = max_jerk[axis] * block->nominal_speed; + if (jerk > mjerk) { + safe_speed *= mjerk / jerk; + limited = true; + } + } else { + safe_speed = max_jerk[axis]; + limited = true; + } + } + } + + // Reset the block flag. + block->flag = 0; + + // Initial limit on the segment entry velocity. + float vmax_junction; //FIXME Vojtech: Why only if at least two lines are planned in the queue? // Is it because we don't want to tinker with the first buffer line, which // is likely to be executed by the stepper interrupt routine soon? if (moves_queued > 1 && previous_nominal_speed > 0.0001f) { -#if 1 - float jerk; - { - float dx = current_speed[X_AXIS]-previous_speed[X_AXIS]; - float dy = current_speed[Y_AXIS]-previous_speed[Y_AXIS]; - jerk = sqrt(dx*dx+dy*dy); - } - float vmax_junction_factor = 1.0; - // if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) { - vmax_junction = block->nominal_speed; - // } - if (jerk > max_xy_jerk) - vmax_junction_factor = max_xy_jerk/jerk; - jerk = fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]); - if (jerk > max_z_jerk) - vmax_junction_factor = min(vmax_junction_factor, max_z_jerk/jerk); - jerk = fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]); - if (jerk > max_e_jerk) - vmax_junction_factor = min(vmax_junction_factor, max_e_jerk/jerk); - //FIXME Vojtech: Why is this asymmetric in regard to the previous nominal speed and the current nominal speed? - vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed -#else // 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. @@ -926,72 +922,54 @@ Having the real displacement of the head, we can calculate the total movement le // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting. vmax_junction = prev_speed_larger ? block->nominal_speed : previous_nominal_speed; // Factor to multiply the previous / current nominal velocities to get componentwise limited velocities. - float v_factor_exit = prev_speed_larger ? smaller_speed_factor : 1.f; - float v_factor_entry = prev_speed_larger ? 1.f : smaller_speed_factor; - // First limit the jerk in the XY plane. - float jerk; - { - // Estimate the jerk as if the entry / exit velocity of the two successive segment was limited to the minimum of their nominal velocities. - // If coasting, then the segment transition velocity will define the exit / entry velocities of the successive segments - // and the jerk defined by the following formula will be always lower. - float dx = prev_speed_larger ? (current_speed[X_AXIS] - smaller_speed_factor * previous_speed[X_AXIS]) : (smaller_speed_factor * current_speed[X_AXIS] - previous_speed[X_AXIS]); - float dy = prev_speed_larger ? (current_speed[Y_AXIS] - smaller_speed_factor * previous_speed[Y_AXIS]) : (smaller_speed_factor * current_speed[Y_AXIS] - previous_speed[Y_AXIS]); - jerk = sqrt(dx*dx+dy*dy); - } - if (jerk > max_xy_jerk) { - // Limit the entry / exit velocities to respect the XY jerk limits. - v_factor_exit = v_factor_entry = max_xy_jerk / jerk; + float v_factor = 1.f; + limited = false; + // Now limit the jerk in all axes. + for (uint8_t axis = 0; axis < 4; ++ axis) { + // Limit an axis. We have to differentiate coasting from the reversal of an axis movement, or a full stop. + float v_exit = previous_speed[axis]; + float v_entry = current_speed [axis]; if (prev_speed_larger) - v_factor_exit *= smaller_speed_factor; - else - v_factor_entry *= smaller_speed_factor; + v_exit *= smaller_speed_factor; + if (limited) { + v_exit *= v_factor; + v_entry *= v_factor; + } + // Calculate the jerk depending on whether the axis is coasting in the same direction or reversing a direction. + float jerk = + (v_exit > v_entry) ? + ((v_entry > 0.f || v_exit < 0.f) ? + // coasting + (v_exit - v_entry) : + // axis reversal + max(v_exit, - v_entry)) : + // v_exit <= v_entry + ((v_entry < 0.f || v_exit > 0.f) ? + // coasting + (v_entry - v_exit) : + // axis reversal + max(- v_exit, v_entry)); + if (jerk > max_jerk[axis]) { + v_factor *= max_jerk[axis] / jerk; + limited = true; + } } - // Now limit the Z and E axes. We have to differentiate coasting from the reversal of an axis movement, or a full stop. - float v_exit = previous_speed[Z_AXIS] * v_factor_exit; - float v_entry = current_speed [Z_AXIS] * v_factor_entry; - jerk = (v_exit > v_entry) ? - ((v_entry > 0.f || v_exit < 0.f) ? - // coasting - (v_exit - v_entry) : - // axis reversal - max(v_exit, - v_entry)) : - // v_exit <= v_entry - ((v_entry < 0.f || v_exit > 0.f) ? - // coasting - (v_entry - v_exit) : - // axis reversal - max(- v_exit, v_entry)); - if (jerk > max_z_jerk / 2.f) { - float c = (max_z_jerk / 2.f) / jerk; - v_factor_exit *= c; - v_factor_entry *= c; + 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. + float vmax_junction_threshold = vmax_junction * 0.99f; + if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold) { + // Not coasting. The machine will stop and start the movements anyway, + // better to start the segment from start. + block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT; + vmax_junction = safe_speed; } - // Limit the E axis. - v_exit = previous_speed[E_AXIS] * v_factor_exit; - v_entry = current_speed [E_AXIS] * v_factor_entry; - jerk = (v_exit > v_entry) ? - ((v_entry > 0.f || v_exit < 0.f) ? - // coasting - (v_exit - v_entry) : - // axis reversal - max(v_exit, - v_entry)) : - // v_exit <= v_entry - ((v_entry < 0.f || v_exit > 0.f) ? - // coasting - (v_entry - v_exit) : - // axis reversal - max(- v_exit, v_entry)); - if (jerk > max_e_jerk / 2.f) { - float c = (max_e_jerk / 2.f) / jerk; - v_factor_exit *= c; - v_factor_entry *= c; - } - - // Now the transition velocity is known as nominal * v_factor. Compare the transition velocity against the "safe" velocoties. - // If the transition velocity is below the exit / enter safe velocity, the machine is no more cruising, therefore - // the safe velocities shall be used. -#endif + } else { + block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT; + vmax_junction = safe_speed; } + // Max entry speed of this block equals the max exit speed of the previous block. block->max_entry_speed = vmax_junction; @@ -1008,12 +986,12 @@ Having the real displacement of the head, we can calculate the total movement le // 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. // Always calculate trapezoid for new block - block->flag = (block->nominal_speed <= v_allowable) ? (BLOCK_FLAG_NOMINAL_LENGTH | BLOCK_FLAG_RECALCULATE) : BLOCK_FLAG_RECALCULATE; + block->flag |= (block->nominal_speed <= v_allowable) ? (BLOCK_FLAG_NOMINAL_LENGTH | BLOCK_FLAG_RECALCULATE) : BLOCK_FLAG_RECALCULATE; // Update previous path unit_vector and nominal speed memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] previous_nominal_speed = block->nominal_speed; - + previous_safe_speed = safe_speed; #ifdef ADVANCE // Calculate advance rate @@ -1056,6 +1034,10 @@ Having the real displacement of the head, we can calculate the total movement le // interfere with the process. planner_recalculate(safe_speed); +// SERIAL_ECHOPGM("Q"); +// SERIAL_ECHO(int(moves_planned())); +// SERIAL_ECHOLNPGM(""); + st_wake_up(); } diff --git a/Firmware/planner.h b/Firmware/planner.h index 28d654aa..268c0c87 100644 --- a/Firmware/planner.h +++ b/Firmware/planner.h @@ -37,12 +37,9 @@ enum BlockFlag { // Planner flag for nominal speed always reached. That means, the segment is long enough, that the nominal speed // may be reached if accelerating from a safe speed (in the regard of jerking from zero speed). BLOCK_FLAG_NOMINAL_LENGTH = 2, - // If set, the machine will stop to a full halt at the end of this block, - // respecting the maximum allowed jerk. - BLOCK_FLAG_FULL_HALT_AT_END = 4, // If set, the machine will start from a halt at the start of this block, // respecting the maximum allowed jerk. - BLOCK_FLAG_START_FROM_FULL_HALT = 8, + BLOCK_FLAG_START_FROM_FULL_HALT = 4, }; // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in @@ -135,9 +132,8 @@ extern unsigned long max_acceleration_units_per_sq_second[NUM_AXIS]; // Use M201 extern float minimumfeedrate; extern float acceleration; // Normal acceleration mm/s^2 THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX extern float retract_acceleration; // mm/s^2 filament pull-pack and push-forward while standing still in the other axis M204 TXXXX -extern float max_xy_jerk; //speed than can be stopped at once, if i understand correctly. -extern float max_z_jerk; -extern float max_e_jerk; +// Jerk is a maximum immediate velocity change. +extern float max_jerk[NUM_AXIS]; extern float mintravelfeedrate; extern unsigned long axis_steps_per_sqr_second[NUM_AXIS];