From c6946084505149b2d20e0129eb13130f7a6af53d Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Thu, 21 Dec 2017 21:43:39 -0600
Subject: [PATCH 1/2] Apply SCARA_FEEDRATE_SCALING to G2/G3
---
Marlin/src/gcode/motion/G2_G3.cpp | 51 ++++++++++++++++++++++++-------
Marlin/src/module/motion.cpp | 29 ++++++------------
2 files changed, 50 insertions(+), 30 deletions(-)
diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp
index c369be034a..921a334500 100644
--- a/Marlin/src/gcode/motion/G2_G3.cpp
+++ b/Marlin/src/gcode/motion/G2_G3.cpp
@@ -29,6 +29,12 @@
#include "../../module/planner.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
#undef N_ARC_CORRECTION
#define N_ARC_CORRECTION 1
@@ -113,7 +119,7 @@ void plan_arc(
* This is important when there are successive arc motions.
*/
// Vector rotation matrix values
- float arc_target[XYZE];
+ float raw[XYZE];
const float theta_per_segment = angular_travel / segments,
linear_per_segment = linear_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
// Initialize the linear axis
- arc_target[l_axis] = current_position[l_axis];
+ raw[l_axis] = current_position[l_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);
@@ -134,6 +140,14 @@ void plan_arc(
int8_t arc_recalc_count = N_ARC_CORRECTION;
#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
thermalManager.manage_heater();
@@ -165,19 +179,34 @@ void plan_arc(
r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti;
}
- // Update arc_target location
- arc_target[p_axis] = center_P + r_P;
- arc_target[q_axis] = center_Q + r_Q;
- arc_target[l_axis] += linear_per_segment;
- arc_target[E_AXIS] += extruder_per_segment;
+ // Update raw location
+ raw[p_axis] = center_P + r_P;
+ raw[q_axis] = center_Q + r_Q;
+ raw[l_axis] += linear_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.
- planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder);
+ #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);
+ #endif
// 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
diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp
index 731335356d..11803c184b 100644
--- a/Marlin/src/module/motion.cpp
+++ b/Marlin/src/module/motion.cpp
@@ -587,7 +587,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
// SERIAL_ECHOPAIR(" seconds=", seconds);
// 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
const float inv_segment_length = min(10.0, float(segments) / cartesian_mm), // 1/mm/segs
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];
+
#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
- #if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
+ #if ENABLED(SCARA_FEEDRATE_SCALING)
// 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], raw[E_AXIS], max(adiff, bdiff) * inverse_secs, active_extruder);
- oldA = delta[A_AXIS];
- oldB = delta[B_AXIS];
+ // i.e., Complete the angular vector in the given time.
+ 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(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
}
- // Since segment_distance is only approximate,
- // the final move must be to the exact destination.
-
- #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
+ // Ensure last segment arrives at target location.
+ #if ENABLED(SCARA_FEEDRATE_SCALING)
inverse_kinematics(rtarget);
ADJUST_DELTA(rtarget);
- 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], raw[E_AXIS], max(adiff, bdiff) * inverse_secs, active_extruder);
+ 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);
#else
planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
#endif
From caa50934984692e7b0de8aeb05438be9a19ce660 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Thu, 21 Dec 2017 22:41:57 -0600
Subject: [PATCH 2/2] General DELTA_IK macro
---
Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp | 2 +-
Marlin/src/module/delta.cpp | 2 +-
Marlin/src/module/delta.h | 18 +++++++++---------
Marlin/src/module/motion.cpp | 2 +-
4 files changed, 12 insertions(+), 12 deletions(-)
diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
index 4bc106976d..fefff867f8 100644
--- a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
+++ b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
@@ -427,7 +427,7 @@
#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);
#elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
diff --git a/Marlin/src/module/delta.cpp b/Marlin/src/module/delta.cpp
index eae9c557aa..e25c29ef18 100644
--- a/Marlin/src/module/delta.cpp
+++ b/Marlin/src/module/delta.cpp
@@ -121,7 +121,7 @@ void recalc_delta_settings() {
}while(0)
void inverse_kinematics(const float raw[XYZ]) {
- DELTA_RAW_IK();
+ DELTA_IK(raw);
// DELTA_DEBUG();
}
diff --git a/Marlin/src/module/delta.h b/Marlin/src/module/delta.h
index fc98f9e6ba..59c01981fc 100644
--- a/Marlin/src/module/delta.h
+++ b/Marlin/src/module/delta.h
@@ -76,17 +76,17 @@ void recalc_delta_settings();
#endif
// Macro to obtain the Z position of an individual tower
-#define DELTA_Z(T) raw[Z_AXIS] + _SQRT( \
- delta_diagonal_rod_2_tower[T] - HYPOT2( \
- delta_tower[T][X_AXIS] - raw[X_AXIS], \
- delta_tower[T][Y_AXIS] - raw[Y_AXIS] \
- ) \
+#define DELTA_Z(V,T) V[Z_AXIS] + _SQRT( \
+ delta_diagonal_rod_2_tower[T] - HYPOT2( \
+ delta_tower[T][X_AXIS] - V[X_AXIS], \
+ delta_tower[T][Y_AXIS] - V[Y_AXIS] \
+ ) \
)
-#define DELTA_RAW_IK() do { \
- delta[A_AXIS] = DELTA_Z(A_AXIS); \
- delta[B_AXIS] = DELTA_Z(B_AXIS); \
- delta[C_AXIS] = DELTA_Z(C_AXIS); \
+#define DELTA_IK(V) do { \
+ delta[A_AXIS] = DELTA_Z(V, A_AXIS); \
+ delta[B_AXIS] = DELTA_Z(V, B_AXIS); \
+ delta[C_AXIS] = DELTA_Z(V, C_AXIS); \
}while(0)
void inverse_kinematics(const float raw[XYZ]);
diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp
index 11803c184b..b13438dab5 100644
--- a/Marlin/src/module/motion.cpp
+++ b/Marlin/src/module/motion.cpp
@@ -613,7 +613,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
LOOP_XYZE(i) raw[i] += segment_distance[i];
#if ENABLED(DELTA)
- DELTA_RAW_IK(); // Delta can inline its kinematics
+ DELTA_IK(raw); // Delta can inline its kinematics
#else
inverse_kinematics(raw);
#endif