andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Junction Deviation jerk limiting option

pull/1/head
Scott Lahteine 7 years ago
parent
0436e16fb2
commit
4d4bf7897d
4 changed files with 152 additions and 103 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      .travis.yml
  2. 9
      Marlin/Configuration_adv.h
  3. 9
      Marlin/src/config/default/Configuration_adv.h
  4. 235
      Marlin/src/module/planner.cpp

2
.travis.yml
View File

@ -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

9
Marlin/Configuration_adv.h
View File

@ -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]

9
Marlin/src/config/default/Configuration_adv.h
View File

@ -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]

235
Marlin/src/module/planner.cpp
View File

@ -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;

Write Preview
Loading…
Cancel
Save