Merge pull request #8865 from thinkyhead/bf2_more_scara_scaling
[2.0.x] SCARA Feedrate Scaling for G2/G3 - using HYPOT
This commit is contained in:
commit
869c89d83f
@ -427,7 +427,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)
|
||||||
|
@ -29,6 +29,12 @@
|
|||||||
#include "../../module/planner.h"
|
#include "../../module/planner.h"
|
||||||
#include "../../module/temperature.h"
|
#include "../../module/temperature.h"
|
||||||
|
|
||||||
|
#if ENABLED(DELTA)
|
||||||
|
#include "../../module/delta.h"
|
||||||
|
#elif ENABLED(SCARA)
|
||||||
|
#include "../../module/scara.h"
|
||||||
|
#endif
|
||||||
|
|
||||||
#if N_ARC_CORRECTION < 1
|
#if N_ARC_CORRECTION < 1
|
||||||
#undef N_ARC_CORRECTION
|
#undef N_ARC_CORRECTION
|
||||||
#define N_ARC_CORRECTION 1
|
#define N_ARC_CORRECTION 1
|
||||||
@ -113,7 +119,7 @@ void plan_arc(
|
|||||||
* This is important when there are successive arc motions.
|
* This is important when there are successive arc motions.
|
||||||
*/
|
*/
|
||||||
// Vector rotation matrix values
|
// Vector rotation matrix values
|
||||||
float arc_target[XYZE];
|
float raw[XYZE];
|
||||||
const float theta_per_segment = angular_travel / segments,
|
const float theta_per_segment = angular_travel / segments,
|
||||||
linear_per_segment = linear_travel / segments,
|
linear_per_segment = linear_travel / segments,
|
||||||
extruder_per_segment = extruder_travel / segments,
|
extruder_per_segment = extruder_travel / segments,
|
||||||
@ -121,10 +127,10 @@ void plan_arc(
|
|||||||
cos_T = 1 - 0.5 * sq(theta_per_segment); // Small angle approximation
|
cos_T = 1 - 0.5 * sq(theta_per_segment); // Small angle approximation
|
||||||
|
|
||||||
// Initialize the linear axis
|
// Initialize the linear axis
|
||||||
arc_target[l_axis] = current_position[l_axis];
|
raw[l_axis] = current_position[l_axis];
|
||||||
|
|
||||||
// Initialize the extruder axis
|
// Initialize the extruder axis
|
||||||
arc_target[E_AXIS] = current_position[E_AXIS];
|
raw[E_AXIS] = current_position[E_AXIS];
|
||||||
|
|
||||||
const float fr_mm_s = MMS_SCALED(feedrate_mm_s);
|
const float fr_mm_s = MMS_SCALED(feedrate_mm_s);
|
||||||
|
|
||||||
@ -134,6 +140,14 @@ void plan_arc(
|
|||||||
int8_t arc_recalc_count = N_ARC_CORRECTION;
|
int8_t arc_recalc_count = N_ARC_CORRECTION;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if ENABLED(SCARA_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 = stepper.get_axis_position_degrees(A_AXIS),
|
||||||
|
oldB = stepper.get_axis_position_degrees(B_AXIS);
|
||||||
|
#endif
|
||||||
|
|
||||||
for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
|
for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
|
||||||
|
|
||||||
thermalManager.manage_heater();
|
thermalManager.manage_heater();
|
||||||
@ -165,19 +179,34 @@ void plan_arc(
|
|||||||
r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti;
|
r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update arc_target location
|
// Update raw location
|
||||||
arc_target[p_axis] = center_P + r_P;
|
raw[p_axis] = center_P + r_P;
|
||||||
arc_target[q_axis] = center_Q + r_Q;
|
raw[q_axis] = center_Q + r_Q;
|
||||||
arc_target[l_axis] += linear_per_segment;
|
raw[l_axis] += linear_per_segment;
|
||||||
arc_target[E_AXIS] += extruder_per_segment;
|
raw[E_AXIS] += extruder_per_segment;
|
||||||
|
|
||||||
clamp_to_software_endstops(arc_target);
|
clamp_to_software_endstops(raw);
|
||||||
|
|
||||||
planner.buffer_line_kinematic(arc_target, fr_mm_s, active_extruder);
|
#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);
|
||||||
|
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];
|
||||||
|
#else
|
||||||
|
planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder);
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
// Ensure last segment arrives at target location.
|
// Ensure last segment arrives at target location.
|
||||||
|
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
||||||
|
inverse_kinematics(cart);
|
||||||
|
ADJUST_DELTA(cart);
|
||||||
|
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);
|
||||||
|
#else
|
||||||
planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder);
|
planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder);
|
||||||
|
#endif
|
||||||
|
|
||||||
// As far as the parser is concerned, the position is now == target. In reality the
|
// As far as the parser is concerned, the position is now == target. In reality the
|
||||||
// motion control system might still be processing the action and the real tool position
|
// motion control system might still be processing the action and the real tool position
|
||||||
|
@ -121,7 +121,7 @@ void recalc_delta_settings() {
|
|||||||
}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();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -76,17 +76,17 @@ void recalc_delta_settings();
|
|||||||
#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)
|
||||||
|
|
||||||
void inverse_kinematics(const float raw[XYZ]);
|
void inverse_kinematics(const float raw[XYZ]);
|
||||||
|
@ -587,7 +587,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
|
|||||||
// SERIAL_ECHOPAIR(" seconds=", seconds);
|
// SERIAL_ECHOPAIR(" seconds=", seconds);
|
||||||
// SERIAL_ECHOLNPAIR(" segments=", segments);
|
// SERIAL_ECHOLNPAIR(" segments=", segments);
|
||||||
|
|
||||||
#if IS_SCARA && 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
|
||||||
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;
|
||||||
@ -611,38 +611,29 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
|
|||||||
}
|
}
|
||||||
|
|
||||||
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
|
||||||
|
|
||||||
ADJUST_DELTA(raw); // Adjust Z if bed leveling is enabled
|
ADJUST_DELTA(raw); // Adjust Z if bed leveling is enabled
|
||||||
|
|
||||||
#if IS_SCARA && 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 = abs(delta[A_AXIS] - oldA),
|
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);
|
||||||
bdiff = abs(delta[B_AXIS] - oldB);
|
oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
|
||||||
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], max(adiff, bdiff) * inverse_secs, active_extruder);
|
|
||||||
oldA = delta[A_AXIS];
|
|
||||||
oldB = delta[B_AXIS];
|
|
||||||
#else
|
#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], 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 IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
|
|
||||||
// For SCARA scale the feed rate from mm/s to degrees/s
|
|
||||||
// With segments > 1 length is 1 segment, otherwise total length
|
|
||||||
inverse_kinematics(rtarget);
|
inverse_kinematics(rtarget);
|
||||||
ADJUST_DELTA(rtarget);
|
ADJUST_DELTA(rtarget);
|
||||||
const float adiff = abs(delta[A_AXIS] - oldA),
|
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);
|
||||||
bdiff = abs(delta[B_AXIS] - oldB);
|
|
||||||
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], max(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
|
||||||
|
Loading…
Reference in New Issue
Block a user