Add delta feedrate scaling (#11153)

This commit is contained in:
Scott Lahteine 2018-06-30 13:44:27 -05:00 committed by GitHub
parent 444725ae9a
commit 8eaac0dab3
Signed by: GitHub
GPG key ID: 4AEE18F83AFDEB23
13 changed files with 100 additions and 21 deletions

View file

@ -522,6 +522,9 @@
// and processor overload (too many expensive sqrt calls). // and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160 #define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained // After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE //#define DELTA_HOME_TO_SAFE_ZONE

View file

@ -522,6 +522,9 @@
// and processor overload (too many expensive sqrt calls). // and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160 #define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained // After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE //#define DELTA_HOME_TO_SAFE_ZONE

View file

@ -522,6 +522,9 @@
// and processor overload (too many expensive sqrt calls). // and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160 #define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained // After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE //#define DELTA_HOME_TO_SAFE_ZONE

View file

@ -527,6 +527,9 @@
// and processor overload (too many expensive sqrt calls). // and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 200 #define DELTA_SEGMENTS_PER_SECOND 200
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained // After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE //#define DELTA_HOME_TO_SAFE_ZONE

View file

@ -512,6 +512,9 @@
// and processor overload (too many expensive sqrt calls). // and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 200 #define DELTA_SEGMENTS_PER_SECOND 200
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained // After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE //#define DELTA_HOME_TO_SAFE_ZONE

View file

@ -512,6 +512,9 @@
// and processor overload (too many expensive sqrt calls). // and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 200 #define DELTA_SEGMENTS_PER_SECOND 200
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained // After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE //#define DELTA_HOME_TO_SAFE_ZONE

View file

@ -498,6 +498,9 @@
// and processor overload (too many expensive sqrt calls). // and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160 #define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained // After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE //#define DELTA_HOME_TO_SAFE_ZONE

View file

@ -516,6 +516,9 @@
// and processor overload (too many expensive sqrt calls). // and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160 #define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained // After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE //#define DELTA_HOME_TO_SAFE_ZONE

View file

@ -35,7 +35,7 @@
#include "../../module/scara.h" #include "../../module/scara.h"
#endif #endif
#if ENABLED(SCARA_FEEDRATE_SCALING) && ENABLED(AUTO_BED_LEVELING_BILINEAR) #if HAS_FEEDRATE_SCALING && ENABLED(AUTO_BED_LEVELING_BILINEAR)
#include "../../feature/bedlevel/abl/abl.h" #include "../../feature/bedlevel/abl/abl.h"
#endif #endif
@ -141,12 +141,16 @@ void plan_arc(
millis_t next_idle_ms = millis() + 200UL; 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 // SCARA needs to scale the feed rate from mm/s to degrees/s
const float inv_segment_length = 1.0 / (MM_PER_ARC_SEGMENT), const float inv_segment_length = 1.0 / (MM_PER_ARC_SEGMENT),
inverse_secs = inv_segment_length * fr_mm_s; inverse_secs = inv_segment_length * fr_mm_s;
float oldA = planner.position_float[A_AXIS], 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 #endif
#if N_ARC_CORRECTION > 1 #if N_ARC_CORRECTION > 1
@ -192,14 +196,23 @@ void plan_arc(
clamp_to_software_endstops(raw); clamp_to_software_endstops(raw);
#if HAS_FEEDRATE_SCALING
inverse_kinematics(raw);
ADJUST_DELTA(raw);
#endif
#if ENABLED(SCARA_FEEDRATE_SCALING) #if ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s // For SCARA scale the feed rate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time. // 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)) 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; break;
oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; 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 #elif HAS_UBL_AND_CURVES
float pos[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] }; float pos[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
planner.apply_leveling(pos); planner.apply_leveling(pos);
@ -212,12 +225,19 @@ void plan_arc(
} }
// Ensure last segment arrives at target location. // Ensure last segment arrives at target location.
#if ENABLED(SCARA_FEEDRATE_SCALING) #if HAS_FEEDRATE_SCALING
inverse_kinematics(cart); inverse_kinematics(cart);
ADJUST_DELTA(cart); ADJUST_DELTA(cart);
#endif
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB); const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB);
if (diff2) if (diff2)
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], cart[Z_AXIS], cart[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder); 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 #elif HAS_UBL_AND_CURVES
float pos[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] }; float pos[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
planner.apply_leveling(pos); planner.apply_leveling(pos);

View file

@ -1053,6 +1053,7 @@
#define PLANNER_LEVELING (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_SEGMENTED || ENABLED(SKEW_CORRECTION)) #define PLANNER_LEVELING (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_SEGMENTED || ENABLED(SKEW_CORRECTION))
#define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)) #define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
#define HAS_UBL_AND_CURVES (ENABLED(AUTO_BED_LEVELING_UBL) && !PLANNER_LEVELING && (ENABLED(ARC_SUPPORT) || ENABLED(BEZIER_CURVE_SUPPORT))) #define HAS_UBL_AND_CURVES (ENABLED(AUTO_BED_LEVELING_UBL) && !PLANNER_LEVELING && (ENABLED(ARC_SUPPORT) || ENABLED(BEZIER_CURVE_SUPPORT)))
#define HAS_FEEDRATE_SCALING (ENABLED(SCARA_FEEDRATE_SCALING) || ENABLED(DELTA_FEEDRATE_SCALING))
#if ENABLED(AUTO_BED_LEVELING_UBL) #if ENABLED(AUTO_BED_LEVELING_UBL)
#undef LCD_BED_LEVELING #undef LCD_BED_LEVELING

View file

@ -581,7 +581,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
ediff * inv_segments ediff * inv_segments
}; };
#if DISABLED(SCARA_FEEDRATE_SCALING) #if !HAS_FEEDRATE_SCALING
const float cartesian_segment_mm = cartesian_mm * inv_segments; const float cartesian_segment_mm = cartesian_mm * inv_segments;
#endif #endif
@ -589,14 +589,13 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
SERIAL_ECHOPAIR("mm=", cartesian_mm); SERIAL_ECHOPAIR("mm=", cartesian_mm);
SERIAL_ECHOPAIR(" seconds=", seconds); SERIAL_ECHOPAIR(" seconds=", seconds);
SERIAL_ECHOPAIR(" segments=", segments); SERIAL_ECHOPAIR(" segments=", segments);
#if DISABLED(SCARA_FEEDRATE_SCALING) #if !HAS_FEEDRATE_SCALING
SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm); SERIAL_ECHOPAIR(" segment_mm=", cartesian_segment_mm);
#else
SERIAL_EOL();
#endif #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 // SCARA needs to scale the feed rate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time. // i.e., Complete the angular vector in the given time.
const float segment_length = cartesian_mm * inv_segments, const float segment_length = cartesian_mm * inv_segments,
@ -604,7 +603,11 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
inverse_secs = inv_segment_length * _feedrate_mm_s; inverse_secs = inv_segment_length * _feedrate_mm_s;
float oldA = planner.position_float[A_AXIS], 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: "); SERIAL_ECHOPGM("Scaled kinematic move: ");
@ -613,7 +616,11 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
SERIAL_ECHOPAIR(") _feedrate_mm_s=", _feedrate_mm_s); SERIAL_ECHOPAIR(") _feedrate_mm_s=", _feedrate_mm_s);
SERIAL_ECHOPAIR(" inverse_secs=", inverse_secs); SERIAL_ECHOPAIR(" inverse_secs=", inverse_secs);
SERIAL_ECHOPAIR(" oldA=", oldA); 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); safe_delay(5);
//*/ //*/
#endif #endif
@ -654,6 +661,19 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
safe_delay(5); safe_delay(5);
//*/ //*/
oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; 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 #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)) 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; break;
@ -661,17 +681,31 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
} }
// Ensure last segment arrives at target location. // Ensure last segment arrives at target location.
#if ENABLED(SCARA_FEEDRATE_SCALING) #if HAS_FEEDRATE_SCALING
inverse_kinematics(rtarget); inverse_kinematics(rtarget);
ADJUST_DELTA(rtarget); ADJUST_DELTA(rtarget);
#endif
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB); const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB);
if (diff2) { if (diff2) {
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], rtarget[Z_AXIS], rtarget[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder); 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("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_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(); SERIAL_EOL();
safe_delay(5); safe_delay(5);
//*/ //*/

View file

@ -149,7 +149,7 @@ typedef struct {
} block_t; } block_t;
#define HAS_POSITION_FLOAT (ENABLED(LIN_ADVANCE) || ENABLED(SCARA_FEEDRATE_SCALING)) #define HAS_POSITION_FLOAT (ENABLED(LIN_ADVANCE) || HAS_FEEDRATE_SCALING)
#define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1)) #define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))