Add delta feedrate scaling (#11152)
This commit is contained in:
Scott Lahteine
GitHub
parent
a789ab4005
commit
a294ec3836
12 changed files with 99 additions and 19 deletions
|
|
@ -13348,7 +13348,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
|||
ediff * inv_segments
|
||||
};
|
||||
|
||||
#if DISABLED(SCARA_FEEDRATE_SCALING)
|
||||
#if !HAS_FEEDRATE_SCALING
|
||||
const float cartesian_segment_mm = cartesian_mm * inv_segments;
|
||||
#endif
|
||||
|
||||
|
|
@ -13356,14 +13356,13 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
|||
SERIAL_ECHOPAIR("mm=", cartesian_mm);
|
||||
SERIAL_ECHOPAIR(" seconds=", seconds);
|
||||
SERIAL_ECHOPAIR(" segments=", segments);
|
||||
#if DISABLED(SCARA_FEEDRATE_SCALING)
|
||||
SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm);
|
||||
#else
|
||||
SERIAL_EOL();
|
||||
#if !HAS_FEEDRATE_SCALING
|
||||
SERIAL_ECHOPAIR(" segment_mm=", cartesian_segment_mm);
|
||||
#endif
|
||||
SERIAL_EOL();
|
||||
//*/
|
||||
|
||||
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
||||
#if HAS_FEEDRATE_SCALING
|
||||
// SCARA needs to scale the feed rate from mm/s to degrees/s
|
||||
// i.e., Complete the angular vector in the given time.
|
||||
const float segment_length = cartesian_mm * inv_segments,
|
||||
|
|
@ -13371,7 +13370,11 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
|||
inverse_secs = inv_segment_length * _feedrate_mm_s;
|
||||
|
||||
float oldA = planner.position_float[A_AXIS],
|
||||
oldB = planner.position_float[B_AXIS];
|
||||
oldB = planner.position_float[B_AXIS]
|
||||
#if ENABLED(DELTA_FEEDRATE_SCALING)
|
||||
, oldC = planner.position_float[C_AXIS]
|
||||
#endif
|
||||
;
|
||||
|
||||
/*
|
||||
SERIAL_ECHOPGM("Scaled kinematic move: ");
|
||||
|
|
@ -13380,7 +13383,11 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
|||
SERIAL_ECHOPAIR(") _feedrate_mm_s=", _feedrate_mm_s);
|
||||
SERIAL_ECHOPAIR(" inverse_secs=", inverse_secs);
|
||||
SERIAL_ECHOPAIR(" oldA=", oldA);
|
||||
SERIAL_ECHOLNPAIR(" oldB=", oldB);
|
||||
SERIAL_ECHOPAIR(" oldB=", oldB);
|
||||
#if ENABLED(DELTA_FEEDRATE_SCALING)
|
||||
SERIAL_ECHOPAIR(" oldC=", oldC);
|
||||
#endif
|
||||
SERIAL_EOL();
|
||||
safe_delay(5);
|
||||
//*/
|
||||
#endif
|
||||
|
|
@ -13421,6 +13428,19 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
|||
safe_delay(5);
|
||||
//*/
|
||||
oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
|
||||
#elif ENABLED(DELTA_FEEDRATE_SCALING)
|
||||
// For DELTA scale the feed rate from Effector mm/s to Carriage mm/s
|
||||
// i.e., Complete the linear vector in the given time.
|
||||
if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], SQRT(sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC)) * inverse_secs, active_extruder))
|
||||
break;
|
||||
/*
|
||||
SERIAL_ECHO(segments);
|
||||
SERIAL_ECHOPAIR(": X=", raw[X_AXIS]); SERIAL_ECHOPAIR(" Y=", raw[Y_AXIS]);
|
||||
SERIAL_ECHOPAIR(" A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]); SERIAL_ECHOPAIR(" C=", delta[C_AXIS]);
|
||||
SERIAL_ECHOLNPAIR(" F", SQRT(sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC)) * inverse_secs * 60);
|
||||
safe_delay(5);
|
||||
//*/
|
||||
oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; oldC = delta[C_AXIS];
|
||||
#else
|
||||
if (!planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm))
|
||||
break;
|
||||
|
|
@ -13428,16 +13448,31 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
|||
}
|
||||
|
||||
// Ensure last segment arrives at target location.
|
||||
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
||||
#if HAS_FEEDRATE_SCALING
|
||||
inverse_kinematics(rtarget);
|
||||
ADJUST_DELTA(rtarget);
|
||||
#endif
|
||||
|
||||
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
||||
const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB);
|
||||
if (diff2) {
|
||||
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], rtarget[Z_AXIS], rtarget[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder);
|
||||
/*
|
||||
SERIAL_ECHOPAIR("final: A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]);
|
||||
SERIAL_ECHOPAIR(" adiff=", delta[A_AXIS] - oldA); SERIAL_ECHOPAIR(" bdiff=", delta[B_AXIS] - oldB);
|
||||
SERIAL_ECHOLNPAIR(" F", (SQRT(diff2) * inverse_secs) * 60);
|
||||
SERIAL_ECHOLNPAIR(" F", SQRT(diff2) * inverse_secs * 60);
|
||||
SERIAL_EOL();
|
||||
safe_delay(5);
|
||||
//*/
|
||||
}
|
||||
#elif ENABLED(DELTA_FEEDRATE_SCALING)
|
||||
const float diff2 = sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC);
|
||||
if (diff2) {
|
||||
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], rtarget[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder);
|
||||
/*
|
||||
SERIAL_ECHOPAIR("final: A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]); SERIAL_ECHOPAIR(" C=", delta[C_AXIS]);
|
||||
SERIAL_ECHOPAIR(" adiff=", delta[A_AXIS] - oldA); SERIAL_ECHOPAIR(" bdiff=", delta[B_AXIS] - oldB); SERIAL_ECHOPAIR(" cdiff=", delta[C_AXIS] - oldC);
|
||||
SERIAL_ECHOLNPAIR(" F", SQRT(diff2) * inverse_secs * 60);
|
||||
SERIAL_EOL();
|
||||
safe_delay(5);
|
||||
//*/
|
||||
|
|
@ -13723,12 +13758,16 @@ void prepare_move_to_destination() {
|
|||
|
||||
millis_t next_idle_ms = millis() + 200UL;
|
||||
|
||||
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
||||
#if HAS_FEEDRATE_SCALING
|
||||
// SCARA needs to scale the feed rate from mm/s to degrees/s
|
||||
const float inv_segment_length = 1.0 / (MM_PER_ARC_SEGMENT),
|
||||
inverse_secs = inv_segment_length * fr_mm_s;
|
||||
float oldA = planner.position_float[A_AXIS],
|
||||
oldB = planner.position_float[B_AXIS];
|
||||
oldB = planner.position_float[B_AXIS]
|
||||
#if ENABLED(DELTA_FEEDRATE_SCALING)
|
||||
, oldC = planner.position_float[C_AXIS]
|
||||
#endif
|
||||
;
|
||||
#endif
|
||||
|
||||
#if N_ARC_CORRECTION > 1
|
||||
|
|
@ -13774,14 +13813,23 @@ void prepare_move_to_destination() {
|
|||
|
||||
clamp_to_software_endstops(raw);
|
||||
|
||||
#if HAS_FEEDRATE_SCALING
|
||||
inverse_kinematics(raw);
|
||||
ADJUST_DELTA(raw);
|
||||
#endif
|
||||
|
||||
#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.
|
||||
inverse_kinematics(raw);
|
||||
ADJUST_DELTA(raw);
|
||||
if (!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))
|
||||
break;
|
||||
oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
|
||||
#elif ENABLED(DELTA_FEEDRATE_SCALING)
|
||||
// For DELTA scale the feed rate from Effector mm/s to Carriage mm/s
|
||||
// i.e., Complete the linear vector in the given time.
|
||||
if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], SQRT(sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC)) * inverse_secs, active_extruder))
|
||||
break;
|
||||
oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; oldC = delta[C_AXIS];
|
||||
#elif HAS_UBL_AND_CURVES
|
||||
float pos[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
|
||||
planner.apply_leveling(pos);
|
||||
|
|
@ -13794,12 +13842,19 @@ void prepare_move_to_destination() {
|
|||
}
|
||||
|
||||
// Ensure last segment arrives at target location.
|
||||
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
||||
#if HAS_FEEDRATE_SCALING
|
||||
inverse_kinematics(cart);
|
||||
ADJUST_DELTA(cart);
|
||||
#endif
|
||||
|
||||
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
||||
const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB);
|
||||
if (diff2)
|
||||
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], cart[Z_AXIS], cart[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder);
|
||||
#elif ENABLED(DELTA_FEEDRATE_SCALING)
|
||||
const float diff2 = sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC);
|
||||
if (diff2)
|
||||
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder);
|
||||
#elif HAS_UBL_AND_CURVES
|
||||
float pos[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
|
||||
planner.apply_leveling(pos);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue