From 0a23a1bb3b7069f7a8ac2ec9eef106a30c259bae Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 28 Mar 2017 16:37:11 -0500
Subject: [PATCH] Dynamic feedrate on SCARA, converting target mm/s to deg/s
---
Marlin/Marlin_main.cpp | 59 ++++++++++++++++++++++++++++++++----------
1 file changed, 46 insertions(+), 13 deletions(-)
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 8f0a4bcd43..595301dbcf 100755
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -9793,34 +9793,43 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
// gives the number of segments
uint16_t segments = delta_segments_per_second * seconds;
- // For SCARA minimum segment size is 0.5mm
+ // For SCARA minimum segment size is 0.25mm
#if IS_SCARA
- NOMORE(segments, cartesian_mm * 2);
+ NOMORE(segments, cartesian_mm * 4);
#endif
// At least one segment is required
NOLESS(segments, 1);
// The approximate length of each segment
- float segment_distance[XYZE] = {
- difference[X_AXIS] / segments,
- difference[Y_AXIS] / segments,
- difference[Z_AXIS] / segments,
- difference[E_AXIS] / segments
- };
+ const float inv_segments = 1.0 / float(segments),
+ segment_distance[XYZE] = {
+ difference[X_AXIS] * inv_segments,
+ difference[Y_AXIS] * inv_segments,
+ difference[Z_AXIS] * inv_segments,
+ difference[E_AXIS] * inv_segments
+ };
// SERIAL_ECHOPAIR("mm=", cartesian_mm);
// SERIAL_ECHOPAIR(" seconds=", seconds);
// SERIAL_ECHOLNPAIR(" segments=", segments);
- // Drop one segment so the last move is to the exact target.
- // If there's only 1 segment, loops will be skipped entirely.
- --segments;
+ #if IS_SCARA
+ // 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
+ feed_factor = inv_segment_length * _feedrate_mm_s;
+ float oldA = stepper.get_axis_position_degrees(A_AXIS),
+ oldB = stepper.get_axis_position_degrees(B_AXIS);
+ #endif
// Get the logical current position as starting point
float logical[XYZE];
COPY(logical, current_position);
+ // Drop one segment so the last move is to the exact target.
+ // If there's only 1 segment, loops will be skipped entirely.
+ --segments;
+
// Calculate and execute the segments
for (uint16_t s = segments + 1; --s;) {
LOOP_XYZE(i) logical[i] += segment_distance[i];
@@ -9829,13 +9838,37 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
#else
inverse_kinematics(logical);
#endif
+
ADJUST_DELTA(logical); // Adjust Z if bed leveling is enabled
- planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
+
+ #if IS_SCARA
+ // 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
+ const float adiff = abs(delta[A_AXIS] - oldA),
+ bdiff = abs(delta[B_AXIS] - oldB);
+ planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
+ oldA = delta[A_AXIS];
+ oldB = delta[B_AXIS];
+ #else
+ planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
+ #endif
}
// Since segment_distance is only approximate,
// the final move must be to the exact destination.
- planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
+
+ #if IS_SCARA
+ // For SCARA scale the feed rate from mm/s to degrees/s
+ // With segments > 1 length is 1 segment, otherwise total length
+ inverse_kinematics(ltarget);
+ ADJUST_DELTA(logical);
+ const float adiff = abs(delta[A_AXIS] - oldA),
+ bdiff = abs(delta[B_AXIS] - oldB);
+ planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
+ #else
+ planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
+ #endif
+
return true;
}
--
GitLab