Minor speedup for SCARA scaling

And cleanup of Delta IK macros...
This commit is contained in:
Scott Lahteine 2017-12-21 21:58:00 -06:00
parent 6f40d57e14
commit b5677907d0
3 changed files with 22 additions and 34 deletions

View File

@ -321,17 +321,17 @@ void report_current_position();
#endif #endif
// Macro to obtain the Z position of an individual tower // Macro to obtain the Z position of an individual tower
#define DELTA_Z(T) raw[Z_AXIS] + _SQRT( \ #define DELTA_Z(V,T) V[Z_AXIS] + _SQRT( \
delta_diagonal_rod_2_tower[T] - HYPOT2( \ delta_diagonal_rod_2_tower[T] - HYPOT2( \
delta_tower[T][X_AXIS] - raw[X_AXIS], \ delta_tower[T][X_AXIS] - V[X_AXIS], \
delta_tower[T][Y_AXIS] - raw[Y_AXIS] \ delta_tower[T][Y_AXIS] - V[Y_AXIS] \
) \ ) \
) )
#define DELTA_RAW_IK() do { \ #define DELTA_IK(V) do { \
delta[A_AXIS] = DELTA_Z(A_AXIS); \ delta[A_AXIS] = DELTA_Z(V, A_AXIS); \
delta[B_AXIS] = DELTA_Z(B_AXIS); \ delta[B_AXIS] = DELTA_Z(V, B_AXIS); \
delta[C_AXIS] = DELTA_Z(C_AXIS); \ delta[C_AXIS] = DELTA_Z(V, C_AXIS); \
}while(0) }while(0)
#elif IS_SCARA #elif IS_SCARA

View File

@ -12749,7 +12749,7 @@ void ok_to_send() {
}while(0) }while(0)
void inverse_kinematics(const float raw[XYZ]) { void inverse_kinematics(const float raw[XYZ]) {
DELTA_RAW_IK(); DELTA_IK(raw);
// DELTA_DEBUG(); // DELTA_DEBUG();
} }
@ -13186,6 +13186,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
#if ENABLED(SCARA_FEEDRATE_SCALING) #if ENABLED(SCARA_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.
const float inv_segment_length = min(10.0, float(segments) / cartesian_mm), // 1/mm/segs const float inv_segment_length = min(10.0, float(segments) / cartesian_mm), // 1/mm/segs
inverse_secs = inv_segment_length * _feedrate_mm_s; inverse_secs = inv_segment_length * _feedrate_mm_s;
float oldA = stepper.get_axis_position_degrees(A_AXIS), float oldA = stepper.get_axis_position_degrees(A_AXIS),
@ -13209,7 +13210,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
LOOP_XYZE(i) raw[i] += segment_distance[i]; LOOP_XYZE(i) raw[i] += segment_distance[i];
#if ENABLED(DELTA) #if ENABLED(DELTA)
DELTA_RAW_IK(); // Delta can inline its kinematics DELTA_IK(raw); // Delta can inline its kinematics
#else #else
inverse_kinematics(raw); inverse_kinematics(raw);
#endif #endif
@ -13218,23 +13219,19 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
#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
// Use ratio between the length of the move and the larger angle change // i.e., Complete the angular vector in the given time.
const float adiff = FABS(delta[A_AXIS] - oldA), bdiff = FABS(delta[B_AXIS] - oldB); 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_line(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(adiff, bdiff) * inverse_secs, active_extruder);
oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
#else #else
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder); planner.buffer_line(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder);
#endif #endif
} }
// Since segment_distance is only approximate, // Ensure last segment arrives at target location.
// the final move must be to the exact destination.
#if ENABLED(SCARA_FEEDRATE_SCALING) #if ENABLED(SCARA_FEEDRATE_SCALING)
inverse_kinematics(rtarget); inverse_kinematics(rtarget);
ADJUST_DELTA(rtarget); ADJUST_DELTA(rtarget);
const float adiff = FABS(delta[A_AXIS] - oldA), bdiff = FABS(delta[B_AXIS] - oldB); 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_line(delta[A_AXIS], delta[B_AXIS], rtarget[Z_AXIS], rtarget[E_AXIS], HYPOT(adiff, bdiff) * inverse_secs, active_extruder);
#else #else
planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder); planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
#endif #endif
@ -13572,20 +13569,12 @@ void prepare_move_to_destination() {
clamp_to_software_endstops(raw); clamp_to_software_endstops(raw);
#if IS_KINEMATIC
#if ENABLED(DELTA)
DELTA_RAW_IK(); // Delta can inline its kinematics
#else
inverse_kinematics(raw);
#endif
ADJUST_DELTA(raw); // Adjust Z if bed leveling is enabled
#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
// With segments > 1 length is 1 segment, otherwise total length // i.e., Complete the angular vector in the given time.
const float adiff = FABS(delta[A_AXIS] - oldA), bdiff = FABS(delta[B_AXIS] - oldB); inverse_kinematics(raw);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(adiff, bdiff) * inverse_secs, active_extruder); ADJUST_DELTA(raw);
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);
oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
#else #else
planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder); planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder);
@ -13596,8 +13585,7 @@ void prepare_move_to_destination() {
#if ENABLED(SCARA_FEEDRATE_SCALING) #if ENABLED(SCARA_FEEDRATE_SCALING)
inverse_kinematics(cart); inverse_kinematics(cart);
ADJUST_DELTA(cart); ADJUST_DELTA(cart);
const float adiff = FABS(delta[A_AXIS] - oldA), bdiff = FABS(delta[B_AXIS] - oldB); planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], cart[Z_AXIS], cart[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], cart[Z_AXIS], cart[E_AXIS], HYPOT(adiff, bdiff) * inverse_secs, active_extruder);
#else #else
planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder); planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder);
#endif #endif

View File

@ -422,7 +422,7 @@
#if ENABLED(DELTA) // apply delta inverse_kinematics #if ENABLED(DELTA) // apply delta inverse_kinematics
DELTA_RAW_IK(); DELTA_IK(raw);
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder); planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder);
#elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw) #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)