MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Fix G2/G3 P<circles> E and Z motion (#19797)

Browse Source 
Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
This commit is contained in:
uwedamm 2020-10-21 03:26:07 +02:00 committed by Scott Lahteine
parent 9f85a15955
commit 35d75c354e
2 changed files with 30 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Marlin/src/gcode/bedlevel/G26.cpp
View File

@ -136,7 +136,7 @@
#define G26_ERR true
#if ENABLED(ARC_SUPPORT)
void plan_arc(const xyze_pos_t &cart, const ab_float_t &offset, const uint8_t clockwise);
void plan_arc(const xyze_pos_t&, const ab_float_t&, const bool, const uint8_t);
#endif
constexpr float g26_e_axis_feedrate = 0.025;
@ -783,7 +783,7 @@ void GcodeSuite::G26() {
const feedRate_t old_feedrate = feedrate_mm_s;
feedrate_mm_s = PLANNER_XY_FEEDRATE() * 0.1f;
plan_arc(endpoint, arc_offset, false); // Draw a counter-clockwise arc
plan_arc(endpoint, arc_offset, false, 0); // Draw a counter-clockwise arc
feedrate_mm_s = old_feedrate;
destination = current_position;

39
Marlin/src/gcode/motion/G2_G3.cpp
View File

@ -52,7 +52,8 @@
void plan_arc(
const xyze_pos_t &cart, // Destination position
const ab_float_t &offset, // Center of rotation relative to current_position
const uint8_t clockwise // Clockwise?
const bool clockwise, // Clockwise?
const uint8_t circles // Take the scenic route
) {
#if ENABLED(CNC_WORKSPACE_PLANES)
AxisEnum p_axis, q_axis, l_axis;
@ -74,9 +75,7 @@ void plan_arc(
center_Q = current_position[q_axis] - rvec.b,
rt_X = cart[p_axis] - center_P,
rt_Y = cart[q_axis] - center_Q,
start_L = current_position[l_axis],
linear_travel = cart[l_axis] - start_L,
extruder_travel = cart.e - current_position.e;
start_L = current_position[l_axis];
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);
@ -90,13 +89,32 @@ void plan_arc(
if (clockwise) angular_travel -= RADIANS(360);
// Make a circle if the angular rotation is 0 and the target is current position
if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis]) {
if (NEAR_ZERO(angular_travel) && NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) {
angular_travel = RADIANS(360);
#ifdef MIN_ARC_SEGMENTS
min_segments = MIN_ARC_SEGMENTS;
#endif
}
float linear_travel = cart[l_axis] - start_L,
extruder_travel = cart.e - current_position.e;
// If circling around...
if (ENABLED(ARC_P_CIRCLES) && circles) {
const float total_angular = angular_travel + circles * RADIANS(360), // Total rotation with all circles and remainder
part_per_circle = RADIANS(360) / total_angular, // Each circle's part of the total
l_per_circle = linear_travel * part_per_circle, // L movement per circle
e_per_circle = extruder_travel * part_per_circle; // E movement per circle
xyze_pos_t temp_position = current_position; // for plan_arc to compare to current_position
for (uint16_t n = circles; n--;) {
temp_position.e += e_per_circle; // Destination E axis
temp_position[l_axis] += l_per_circle; // Destination L axis
plan_arc(temp_position, offset, clockwise, 0); // Plan a single whole circle
}
linear_travel = cart[l_axis] - current_position[l_axis];
extruder_travel = cart.e - current_position.e;
}
const float flat_mm = radius * angular_travel,
mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm);
if (mm_of_travel < 0.001f) return;
@ -150,7 +168,7 @@ void plan_arc(
linear_per_segment = linear_travel / segments,
extruder_per_segment = extruder_travel / segments,
sq_theta_per_segment = sq(theta_per_segment),
sin_T = theta_per_segment - sq_theta_per_segment*theta_per_segment/6,
sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
// Initialize the linear axis
@ -320,16 +338,15 @@ void GcodeSuite::G2_G3(const bool clockwise) {
#if ENABLED(ARC_P_CIRCLES)
// P indicates number of circles to do
int8_t circles_to_do = parser.byteval('P');
const int8_t circles_to_do = parser.byteval('P');
if (!WITHIN(circles_to_do, 0, 100))
SERIAL_ERROR_MSG(STR_ERR_ARC_ARGS);
while (circles_to_do--)
plan_arc(current_position, arc_offset, clockwise);
#else
constexpr uint8_t circles_to_do = 0;
#endif
// Send the arc to the planner
plan_arc(destination, arc_offset, clockwise);
plan_arc(destination, arc_offset, clockwise, circles_to_do);
reset_stepper_timeout();
}
else