3DPrinting/Prusa-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Reworked the calculation of jerks in the planner.

Browse Source 
Now the confugration values are half the values used before,
and the planner ensures, that the jerks will not be violated.
This commit is contained in:
bubnikv 2016-07-22 16:52:13 +02:00
parent 986b286803
commit d00b4a2c75
5 changed files with 102 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Firmware/Configuration.h
View File

@ -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 // #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) // 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_XJERK 10.0 // (mm/sec)
#define DEFAULT_ZJERK 0.4 // (mm/sec) #define DEFAULT_YJERK 10.0 // (mm/sec)
#define DEFAULT_EJERK 5.0 // (mm/sec) #define DEFAULT_ZJERK 0.2 // (mm/sec)
#define DEFAULT_EJERK 2.5 // (mm/sec)
//=========================================================================== //===========================================================================
//=============================Additional Features=========================== //=============================Additional Features===========================

14
Firmware/ConfigurationStore.cpp
View File

@ -169,9 +169,10 @@ void Config_PrintSettings()
SERIAL_ECHOPAIR(" M205 S",minimumfeedrate ); SERIAL_ECHOPAIR(" M205 S",minimumfeedrate );
SERIAL_ECHOPAIR(" T" ,mintravelfeedrate ); SERIAL_ECHOPAIR(" T" ,mintravelfeedrate );
SERIAL_ECHOPAIR(" B" ,minsegmenttime ); SERIAL_ECHOPAIR(" B" ,minsegmenttime );
SERIAL_ECHOPAIR(" X" ,max_xy_jerk ); SERIAL_ECHOPAIR(" X" ,max_jerk[X_AXIS] );
SERIAL_ECHOPAIR(" Z" ,max_z_jerk); SERIAL_ECHOPAIR(" Y" ,max_jerk[Y_AXIS] );
SERIAL_ECHOPAIR(" E" ,max_e_jerk); SERIAL_ECHOPAIR(" Z" ,max_jerk[Z_AXIS] );
SERIAL_ECHOPAIR(" E" ,max_jerk[E_AXIS] );
SERIAL_ECHOLN(""); SERIAL_ECHOLN("");
SERIAL_ECHO_START; SERIAL_ECHO_START;
@ -356,9 +357,10 @@ void Config_ResetDefault()
minimumfeedrate=DEFAULT_MINIMUMFEEDRATE; minimumfeedrate=DEFAULT_MINIMUMFEEDRATE;
minsegmenttime=DEFAULT_MINSEGMENTTIME; minsegmenttime=DEFAULT_MINSEGMENTTIME;
mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE; mintravelfeedrate=DEFAULT_MINTRAVELFEEDRATE;
max_xy_jerk=DEFAULT_XYJERK; max_jerk[X_AXIS] = DEFAULT_XJERK;
max_z_jerk=DEFAULT_ZJERK; max_jerk[Y_AXIS] = DEFAULT_YJERK;
max_e_jerk=DEFAULT_EJERK; 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; add_homing[X_AXIS] = add_homing[Y_AXIS] = add_homing[Z_AXIS] = 0;
#ifdef ULTIPANEL #ifdef ULTIPANEL
plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP; plaPreheatHotendTemp = PLA_PREHEAT_HOTEND_TEMP;

9
Firmware/Marlin_main.cpp
View File

@ -3515,7 +3515,7 @@ Sigma_Exit:
float value = code_value(); float value = code_value();
if(value < 20.0) { if(value < 20.0) {
float factor = axis_steps_per_unit[i] / value; // increase e constants if M92 E14 is given for netfab. 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; max_feedrate[i] *= factor;
axis_steps_per_sqr_second[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('S')) minimumfeedrate = code_value();
if(code_seen('T')) mintravelfeedrate = code_value(); if(code_seen('T')) mintravelfeedrate = code_value();
if(code_seen('B')) minsegmenttime = code_value() ; if(code_seen('B')) minsegmenttime = code_value() ;
if(code_seen('X')) max_xy_jerk = code_value() ; if(code_seen('X')) max_jerk[X_AXIS] = max_jerk[Y_AXIS] = code_value();
if(code_seen('Z')) max_z_jerk = code_value() ; if(code_seen('Y')) max_jerk[Y_AXIS] = code_value();
if(code_seen('E')) max_e_jerk = code_value() ; if(code_seen('Z')) max_jerk[Z_AXIS] = code_value();
if(code_seen('E')) max_jerk[E_AXIS] = code_value();
} }
break; break;
case 206: // M206 additional homing offset case 206: // M206 additional homing offset

182
Firmware/planner.cpp
View File

@ -74,9 +74,8 @@ unsigned long max_acceleration_units_per_sq_second[NUM_AXIS]; // Use M201 to ove
float minimumfeedrate; float minimumfeedrate;
float acceleration; // Normal acceleration mm/s^2 THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX 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 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. // Jerk is a maximum immediate velocity change.
float max_z_jerk; float max_jerk[NUM_AXIS];
float max_e_jerk;
float mintravelfeedrate; float mintravelfeedrate;
unsigned long axis_steps_per_sqr_second[NUM_AXIS]; 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 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_speed[NUM_AXIS]; // Speed of previous path line segment
static float previous_nominal_speed; // Nominal 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 #ifdef AUTOTEMP
float autotemp_max=250; float autotemp_max=250;
@ -279,7 +279,7 @@ void planner_recalculate(const float &safe_final_speed)
tail = block_index; tail = block_index;
// Update the number of blocks to process. // Update the number of blocks to process.
n_blocks = (block_buffer_head + BLOCK_BUFFER_SIZE - tail) & (BLOCK_BUFFER_SIZE - 1); 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; break;
} }
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. // 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 #endif
// Start with a safe speed. // 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. // 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? //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 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? // is likely to be executed by the stepper interrupt routine soon?
if (moves_queued > 1 && previous_nominal_speed > 0.0001f) { 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. // 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, // 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. // 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. // 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; vmax_junction = prev_speed_larger ? block->nominal_speed : previous_nominal_speed;
// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities. // 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 = 1.f;
float v_factor_entry = prev_speed_larger ? 1.f : smaller_speed_factor; limited = false;
// First limit the jerk in the XY plane. // Now limit the jerk in all axes.
float jerk; 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.
// Estimate the jerk as if the entry / exit velocity of the two successive segment was limited to the minimum of their nominal velocities. float v_exit = previous_speed[axis];
// If coasting, then the segment transition velocity will define the exit / entry velocities of the successive segments float v_entry = current_speed [axis];
// 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;
if (prev_speed_larger) if (prev_speed_larger)
v_factor_exit *= smaller_speed_factor; v_exit *= smaller_speed_factor;
else if (limited) {
v_factor_entry *= smaller_speed_factor; 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. if (limited)
float v_exit = previous_speed[Z_AXIS] * v_factor_exit; vmax_junction *= v_factor;
float v_entry = current_speed [Z_AXIS] * v_factor_entry; // Now the transition velocity is known, which maximizes the shared exit / entry velocity while
jerk = (v_exit > v_entry) ? // respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints.
((v_entry > 0.f || v_exit < 0.f) ? float vmax_junction_threshold = vmax_junction * 0.99f;
// coasting if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold) {
(v_exit - v_entry) : // Not coasting. The machine will stop and start the movements anyway,
// axis reversal // better to start the segment from start.
max(v_exit, - v_entry)) : block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT;
// v_exit <= v_entry vmax_junction = safe_speed;
((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;
} }
// Limit the E axis. } else {
v_exit = previous_speed[E_AXIS] * v_factor_exit; block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT;
v_entry = current_speed [E_AXIS] * v_factor_entry; vmax_junction = safe_speed;
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
} }
// Max entry speed of this block equals the max exit speed of the previous block. // Max entry speed of this block equals the max exit speed of the previous block.
block->max_entry_speed = vmax_junction; 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 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.
// Always calculate trapezoid for new block // 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 // Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
previous_nominal_speed = block->nominal_speed; previous_nominal_speed = block->nominal_speed;
previous_safe_speed = safe_speed;
#ifdef ADVANCE #ifdef ADVANCE
// Calculate advance rate // 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. // interfere with the process.
planner_recalculate(safe_speed); planner_recalculate(safe_speed);
// SERIAL_ECHOPGM("Q");
// SERIAL_ECHO(int(moves_planned()));
// SERIAL_ECHOLNPGM("");
st_wake_up(); st_wake_up();
} }

10
Firmware/planner.h
View File

@ -37,12 +37,9 @@ enum BlockFlag {
// Planner flag for nominal speed always reached. That means, the segment is long enough, that the nominal speed // 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). // may be reached if accelerating from a safe speed (in the regard of jerking from zero speed).
BLOCK_FLAG_NOMINAL_LENGTH = 2, 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, // If set, the machine will start from a halt at the start of this block,
// respecting the maximum allowed jerk. // 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 // 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 minimumfeedrate;
extern float acceleration; // Normal acceleration mm/s^2 THIS IS THE DEFAULT ACCELERATION for all moves. M204 SXXXX 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 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. // Jerk is a maximum immediate velocity change.
extern float max_z_jerk; extern float max_jerk[NUM_AXIS];
extern float max_e_jerk;
extern float mintravelfeedrate; extern float mintravelfeedrate;
extern unsigned long axis_steps_per_sqr_second[NUM_AXIS]; extern unsigned long axis_steps_per_sqr_second[NUM_AXIS];