bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

[2.0.x] Apply feedrate to nozzle movement for kinematic machines (#8778)

Browse Source
This commit is contained in:
Thomas Moore
2018-02-04 00:26:05 -06:00
committed by Scott Lahteine
parent 5364b92c37
commit 786746404b
41 changed files with 763 additions and 328 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
Marlin/src/module/motion.cpp
View File

@ -580,6 +580,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
// The approximate length of each segment
const float inv_segments = 1.0 / float(segments),
cartesian_segment_mm = cartesian_mm * inv_segments,
segment_distance[XYZE] = {
xdiff * inv_segments,
ydiff * inv_segments,
@ -590,6 +591,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
// SERIAL_ECHOPAIR("mm=", cartesian_mm);
// SERIAL_ECHOPAIR(" seconds=", seconds);
// SERIAL_ECHOLNPAIR(" segments=", segments);
// SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm);
#if ENABLED(SCARA_FEEDRATE_SCALING)
// SCARA needs to scale the feed rate from mm/s to degrees/s
@ -626,10 +628,10 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
#if ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time.
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder);
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder, cartesian_segment_mm);
oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
#else
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm);
#endif
}
@ -637,9 +639,9 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
#if ENABLED(SCARA_FEEDRATE_SCALING)
inverse_kinematics(rtarget);
ADJUST_DELTA(rtarget);
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], rtarget[Z_AXIS], rtarget[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder);
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], rtarget[Z_AXIS], rtarget[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder, cartesian_segment_mm);
#else
planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder, cartesian_segment_mm);
#endif
return false; // caller will update current_position
@ -685,6 +687,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
// The approximate length of each segment
const float inv_segments = 1.0 / float(segments),
cartesian_segment_mm = cartesian_mm * inv_segments,
segment_distance[XYZE] = {
xdiff * inv_segments,
ydiff * inv_segments,
@ -694,6 +697,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
// SERIAL_ECHOPAIR("mm=", cartesian_mm);
// SERIAL_ECHOLNPAIR(" segments=", segments);
// SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm);
// Get the raw current position as starting point
float raw[XYZE];
@ -708,12 +712,12 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
idle();
}
LOOP_XYZE(i) raw[i] += segment_distance[i];
planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder);
planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder, cartesian_segment_mm);
}
// Since segment_distance is only approximate,
// the final move must be to the exact destination.
planner.buffer_line_kinematic(destination, fr_mm_s, active_extruder);
planner.buffer_line_kinematic(destination, fr_mm_s, active_extruder, cartesian_segment_mm);
}
#endif // SEGMENT_LEVELED_MOVES

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

@ -726,11 +726,11 @@ void Planner::check_axes_activity() {
* fr_mm_s - (target) speed of the move
* extruder - target extruder
*/
void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const uint8_t extruder) {
void Planner::_buffer_steps(const int32_t (&target)[ABCE], float fr_mm_s, const uint8_t extruder, const float &millimeters /*= 0.0*/) {
const int32_t da = target[X_AXIS] - position[X_AXIS],
db = target[Y_AXIS] - position[Y_AXIS],
dc = target[Z_AXIS] - position[Z_AXIS];
const int32_t da = target[A_AXIS] - position[A_AXIS],
db = target[B_AXIS] - position[B_AXIS],
dc = target[C_AXIS] - position[C_AXIS];
int32_t de = target[E_AXIS] - position[E_AXIS];
@ -832,13 +832,13 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
block->steps[C_AXIS] = labs(db - dc);
#else
// default non-h-bot planning
block->steps[X_AXIS] = labs(da);
block->steps[Y_AXIS] = labs(db);
block->steps[Z_AXIS] = labs(dc);
block->steps[A_AXIS] = labs(da);
block->steps[B_AXIS] = labs(db);
block->steps[C_AXIS] = labs(dc);
#endif
block->steps[E_AXIS] = esteps;
block->step_event_count = MAX4(block->steps[X_AXIS], block->steps[Y_AXIS], block->steps[Z_AXIS], esteps);
block->step_event_count = MAX4(block->steps[A_AXIS], block->steps[B_AXIS], block->steps[C_AXIS], esteps);
// Bail if this is a zero-length block
if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return;
@ -1019,17 +1019,17 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
delta_mm[C_AXIS] = CORESIGN(db - dc) * steps_to_mm[C_AXIS];
#endif
#else
float delta_mm[XYZE];
delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS];
delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS];
delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS];
float delta_mm[ABCE];
delta_mm[A_AXIS] = da * steps_to_mm[A_AXIS];
delta_mm[B_AXIS] = db * steps_to_mm[B_AXIS];
delta_mm[C_AXIS] = dc * steps_to_mm[C_AXIS];
#endif
delta_mm[E_AXIS] = esteps_float * steps_to_mm[E_AXIS_N];
if (block->steps[X_AXIS] < MIN_STEPS_PER_SEGMENT && block->steps[Y_AXIS] < MIN_STEPS_PER_SEGMENT && block->steps[Z_AXIS] < MIN_STEPS_PER_SEGMENT) {
if (block->steps[A_AXIS] < MIN_STEPS_PER_SEGMENT && block->steps[B_AXIS] < MIN_STEPS_PER_SEGMENT && block->steps[C_AXIS] < MIN_STEPS_PER_SEGMENT) {
block->millimeters = FABS(delta_mm[E_AXIS]);
}
else {
else if (!millimeters) {
block->millimeters = SQRT(
#if CORE_IS_XY
sq(delta_mm[X_HEAD]) + sq(delta_mm[Y_HEAD]) + sq(delta_mm[Z_AXIS])
@ -1042,6 +1042,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
#endif
);
}
else
block->millimeters = millimeters;
const float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate inverse time for this move. No divide by zero due to previous checks.
@ -1170,7 +1173,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
// Compute and limit the acceleration rate for the trapezoid generator.
const float steps_per_mm = block->step_event_count * inverse_millimeters;
uint32_t accel;
if (!block->steps[X_AXIS] && !block->steps[Y_AXIS] && !block->steps[Z_AXIS]) {
if (!block->steps[A_AXIS] && !block->steps[B_AXIS] && !block->steps[C_AXIS]) {
// convert to: acceleration steps/sec^2
accel = CEIL(retract_acceleration * steps_per_mm);
}
@ -1200,15 +1203,15 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
// Limit acceleration per axis
if (block->step_event_count <= cutoff_long) {
LIMIT_ACCEL_LONG(X_AXIS, 0);
LIMIT_ACCEL_LONG(Y_AXIS, 0);
LIMIT_ACCEL_LONG(Z_AXIS, 0);
LIMIT_ACCEL_LONG(A_AXIS, 0);
LIMIT_ACCEL_LONG(B_AXIS, 0);
LIMIT_ACCEL_LONG(C_AXIS, 0);
LIMIT_ACCEL_LONG(E_AXIS, ACCEL_IDX);
}
else {
LIMIT_ACCEL_FLOAT(X_AXIS, 0);
LIMIT_ACCEL_FLOAT(Y_AXIS, 0);
LIMIT_ACCEL_FLOAT(Z_AXIS, 0);
LIMIT_ACCEL_FLOAT(A_AXIS, 0);
LIMIT_ACCEL_FLOAT(B_AXIS, 0);
LIMIT_ACCEL_FLOAT(C_AXIS, 0);
LIMIT_ACCEL_FLOAT(E_AXIS, ACCEL_IDX);
}
}
@ -1225,9 +1228,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
// Compute path unit vector
double unit_vec[XYZ] = {
delta_mm[X_AXIS] * inverse_millimeters,
delta_mm[Y_AXIS] * inverse_millimeters,
delta_mm[Z_AXIS] * inverse_millimeters
delta_mm[A_AXIS] * inverse_millimeters,
delta_mm[B_AXIS] * inverse_millimeters,
delta_mm[C_AXIS] * inverse_millimeters
};
/*
@ -1417,11 +1420,12 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
*
* Leveling and kinematics should be applied ahead of calling this.
*
* a,b,c,e - target positions in mm and/or degrees
* fr_mm_s - (target) speed of the move
* extruder - target extruder
* a,b,c,e - target positions in mm and/or degrees
* fr_mm_s - (target) speed of the move
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
void Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder) {
void Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters /*= 0.0*/) {
// When changing extruders recalculate steps corresponding to the E position
#if ENABLED(DISTINCT_E_FACTORS)
if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) {
@ -1432,10 +1436,10 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
const int32_t target[XYZE] = {
LROUND(a * axis_steps_per_mm[X_AXIS]),
LROUND(b * axis_steps_per_mm[Y_AXIS]),
LROUND(c * axis_steps_per_mm[Z_AXIS]),
const int32_t target[ABCE] = {
LROUND(a * axis_steps_per_mm[A_AXIS]),
LROUND(b * axis_steps_per_mm[B_AXIS]),
LROUND(c * axis_steps_per_mm[C_AXIS]),
LROUND(e * axis_steps_per_mm[E_AXIS_N])
};
@ -1513,7 +1517,7 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
if (!blocks_queued()) {
#define _BETWEEN(A) (position[A##_AXIS] + target[A##_AXIS]) >> 1
const int32_t between[XYZE] = { _BETWEEN(X), _BETWEEN(Y), _BETWEEN(Z), _BETWEEN(E) };
const int32_t between[ABCE] = { _BETWEEN(A), _BETWEEN(B), _BETWEEN(C), _BETWEEN(E) };
DISABLE_STEPPER_DRIVER_INTERRUPT();
#if ENABLED(LIN_ADVANCE)
@ -1521,7 +1525,7 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
lin_dist_e *= 0.5;
#endif
_buffer_steps(between, fr_mm_s, extruder);
_buffer_steps(between, fr_mm_s, extruder, millimeters * 0.5);
#if ENABLED(LIN_ADVANCE)
position_float[X_AXIS] = (position_float[X_AXIS] + a) * 0.5;
@ -1531,12 +1535,12 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
#endif
const uint8_t next = block_buffer_head;
_buffer_steps(target, fr_mm_s, extruder);
_buffer_steps(target, fr_mm_s, extruder, millimeters * 0.5);
SBI(block_buffer[next].flag, BLOCK_BIT_CONTINUED);
ENABLE_STEPPER_DRIVER_INTERRUPT();
}
else
_buffer_steps(target, fr_mm_s, extruder);
_buffer_steps(target, fr_mm_s, extruder, millimeters);
stepper.wake_up();
@ -1562,9 +1566,9 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
#else
#define _EINDEX E_AXIS
#endif
const int32_t na = position[X_AXIS] = LROUND(a * axis_steps_per_mm[X_AXIS]),
nb = position[Y_AXIS] = LROUND(b * axis_steps_per_mm[Y_AXIS]),
nc = position[Z_AXIS] = LROUND(c * axis_steps_per_mm[Z_AXIS]),
const int32_t na = position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]),
nb = position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]),
nc = position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]),
ne = position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
#if ENABLED(LIN_ADVANCE)
position_float[X_AXIS] = a;

30
Marlin/src/module/planner.h
View File

@ -416,8 +416,9 @@ class Planner {
* target - target position in steps units
* fr_mm_s - (target) speed of the move
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
static void _buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const uint8_t extruder);
static void _buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const uint8_t extruder, const float &millimeters = 0.0);
/**
* Planner::buffer_segment
@ -426,11 +427,12 @@ class Planner {
*
* Leveling and kinematics should be applied ahead of calling this.
*
* a,b,c,e - target positions in mm and/or degrees
* fr_mm_s - (target) speed of the move
* extruder - target extruder
* a,b,c,e - target positions in mm and/or degrees
* fr_mm_s - (target) speed of the move
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
static void buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder);
static void buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters = 0.0);
static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
@ -445,12 +447,13 @@ class Planner {
* rx,ry,rz,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
FORCE_INLINE static void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder) {
FORCE_INLINE static void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
#if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(rx, ry, rz);
#endif
buffer_segment(rx, ry, rz, e, fr_mm_s, extruder);
buffer_segment(rx, ry, rz, e, fr_mm_s, extruder, millimeters);
}
/**
@ -458,11 +461,12 @@ class Planner {
* The target is cartesian, it's translated to delta/scara if
* needed.
*
* cart - x,y,z,e CARTESIAN target in mm
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
* cart - x,y,z,e CARTESIAN target in mm
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
FORCE_INLINE static void buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder) {
FORCE_INLINE static void buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
#if PLANNER_LEVELING
float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
apply_leveling(raw);
@ -471,9 +475,9 @@ class Planner {
#endif
#if IS_KINEMATIC
inverse_kinematics(raw);
buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder);
buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
#else
buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder);
buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
#endif
}