Firmware2/Marlin/src/gcode/motion/G2_G3.cpp

343 lines
12 KiB
C++
Raw Normal View History

/**
* Marlin 3D Printer Firmware
2019-06-28 06:57:50 +02:00
* Copyright (c) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
2019-06-28 06:57:50 +02:00
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*/
2017-09-16 03:57:52 +02:00
#include "../../inc/MarlinConfig.h"
#if ENABLED(ARC_SUPPORT)
#include "../gcode.h"
2017-09-16 03:57:52 +02:00
#include "../../module/motion.h"
#include "../../module/planner.h"
#include "../../module/temperature.h"
2017-12-22 04:43:39 +01:00
#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
#endif
/**
* Plan an arc in 2 dimensions
*
* The arc is approximated by generating many small linear segments.
* The length of each segment is configured in MM_PER_ARC_SEGMENT (Default 1mm)
* Arcs should only be made relatively large (over 5mm), as larger arcs with
* larger segments will tend to be more efficient. Your slicer should have
* options for G2/G3 arc generation. In future these options may be GCode tunable.
*/
void plan_arc(
2019-09-29 11:25:39 +02:00
const xyze_pos_t &cart, // Destination position
const ab_float_t &offset, // Center of rotation relative to current_position
const uint8_t clockwise // Clockwise?
) {
#if ENABLED(CNC_WORKSPACE_PLANES)
AxisEnum p_axis, q_axis, l_axis;
2017-09-16 06:08:48 +02:00
switch (gcode.workspace_plane) {
2017-11-07 05:57:05 +01:00
default:
2017-09-16 06:08:48 +02:00
case GcodeSuite::PLANE_XY: p_axis = X_AXIS; q_axis = Y_AXIS; l_axis = Z_AXIS; break;
case GcodeSuite::PLANE_YZ: p_axis = Y_AXIS; q_axis = Z_AXIS; l_axis = X_AXIS; break;
case GcodeSuite::PLANE_ZX: p_axis = Z_AXIS; q_axis = X_AXIS; l_axis = Y_AXIS; break;
}
#else
constexpr AxisEnum p_axis = X_AXIS, q_axis = Y_AXIS, l_axis = Z_AXIS;
#endif
// Radius vector from center to current location
2019-09-29 11:25:39 +02:00
ab_float_t rvec = -offset;
2019-09-29 11:25:39 +02:00
const float radius = HYPOT(rvec.a, rvec.b),
#if ENABLED(AUTO_BED_LEVELING_UBL)
start_L = current_position[l_axis],
#endif
2019-09-29 11:25:39 +02:00
center_P = current_position[p_axis] - rvec.a,
center_Q = current_position[q_axis] - rvec.b,
2017-12-09 09:10:54 +01:00
rt_X = cart[p_axis] - center_P,
rt_Y = cart[q_axis] - center_Q,
linear_travel = cart[l_axis] - current_position[l_axis],
2019-09-29 11:25:39 +02:00
extruder_travel = cart.e - current_position.e;
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
2019-09-29 11:25:39 +02:00
float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);
if (angular_travel < 0) angular_travel += RADIANS(360);
#ifdef MIN_ARC_SEGMENTS
uint16_t min_segments = CEIL((MIN_ARC_SEGMENTS) * (angular_travel / RADIANS(360)));
NOLESS(min_segments, 1U);
#else
constexpr uint16_t min_segments = 1;
#endif
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]) {
angular_travel = RADIANS(360);
#ifdef MIN_ARC_SEGMENTS
min_segments = MIN_ARC_SEGMENTS;
#endif
}
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;
const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
#ifdef ARC_SEGMENTS_PER_SEC
float seg_length = scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC);
NOLESS(seg_length, MM_PER_ARC_SEGMENT);
#else
constexpr float seg_length = MM_PER_ARC_SEGMENT;
#endif
uint16_t segments = FLOOR(mm_of_travel / seg_length);
NOLESS(segments, min_segments);
/**
* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
* and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
* r_T = [cos(phi) -sin(phi);
* sin(phi) cos(phi)] * r ;
*
* For arc generation, the center of the circle is the axis of rotation and the radius vector is
* defined from the circle center to the initial position. Each line segment is formed by successive
* vector rotations. This requires only two cos() and sin() computations to form the rotation
* matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
* all double numbers are single precision on the Arduino. (True double precision will not have
* round off issues for CNC applications.) Single precision error can accumulate to be greater than
* tool precision in some cases. Therefore, arc path correction is implemented.
*
* Small angle approximation may be used to reduce computation overhead further. This approximation
* holds for everything, but very small circles and large MM_PER_ARC_SEGMENT values. In other words,
* theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
* to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
* numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
* issue for CNC machines with the single precision Arduino calculations.
*
* This approximation also allows plan_arc to immediately insert a line segment into the planner
* without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
* a correction, the planner should have caught up to the lag caused by the initial plan_arc overhead.
* This is important when there are successive arc motions.
*/
// Vector rotation matrix values
2019-09-29 11:25:39 +02:00
xyze_pos_t raw;
const float theta_per_segment = angular_travel / segments,
linear_per_segment = linear_travel / segments,
extruder_per_segment = extruder_travel / segments,
sin_T = theta_per_segment,
cos_T = 1 - 0.5f * sq(theta_per_segment); // Small angle approximation
// Initialize the linear axis
2017-12-22 04:43:39 +01:00
raw[l_axis] = current_position[l_axis];
// Initialize the extruder axis
2019-09-29 11:25:39 +02:00
raw.e = current_position.e;
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = scaled_fr_mm_s / seg_length;
#endif
millis_t next_idle_ms = millis() + 200UL;
#if N_ARC_CORRECTION > 1
int8_t arc_recalc_count = N_ARC_CORRECTION;
#endif
for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
thermalManager.manage_heater();
if (ELAPSED(millis(), next_idle_ms)) {
next_idle_ms = millis() + 200UL;
idle();
}
#if N_ARC_CORRECTION > 1
if (--arc_recalc_count) {
2019-09-29 11:25:39 +02:00
// Apply vector rotation matrix to previous rvec.a / 1
const float r_new_Y = rvec.a * sin_T + rvec.b * cos_T;
rvec.a = rvec.a * cos_T - rvec.b * sin_T;
rvec.b = r_new_Y;
}
else
#endif
{
#if N_ARC_CORRECTION > 1
arc_recalc_count = N_ARC_CORRECTION;
#endif
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
// To reduce stuttering, the sin and cos could be computed at different times.
// For now, compute both at the same time.
const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
2019-09-29 11:25:39 +02:00
rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti;
rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti;
}
2017-12-22 04:43:39 +01:00
// Update raw location
2019-09-29 11:25:39 +02:00
raw[p_axis] = center_P + rvec.a;
raw[q_axis] = center_Q + rvec.b;
#if ENABLED(AUTO_BED_LEVELING_UBL)
raw[l_axis] = start_L;
2019-01-20 01:47:29 +01:00
UNUSED(linear_per_segment);
#else
raw[l_axis] += linear_per_segment;
#endif
2019-09-29 11:25:39 +02:00
raw.e += extruder_per_segment;
2017-12-22 04:43:39 +01:00
apply_motion_limits(raw);
2017-12-22 04:43:39 +01:00
#if HAS_LEVELING && !PLANNER_LEVELING
planner.apply_leveling(raw);
2018-06-30 20:44:27 +02:00
#endif
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, seg_length
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
))
break;
}
// Ensure last segment arrives at target location.
2019-09-29 11:25:39 +02:00
raw = cart;
#if ENABLED(AUTO_BED_LEVELING_UBL)
raw[l_axis] = start_L;
#endif
2018-06-30 20:44:27 +02:00
apply_motion_limits(raw);
2019-09-05 04:40:40 +02:00
#if HAS_LEVELING && !PLANNER_LEVELING
planner.apply_leveling(raw);
#endif
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, seg_length
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
#if ENABLED(AUTO_BED_LEVELING_UBL)
raw[l_axis] = start_L;
#endif
2019-09-29 11:25:39 +02:00
current_position = raw;
} // plan_arc
/**
* G2: Clockwise Arc
* G3: Counterclockwise Arc
*
* This command has two forms: IJ-form (JK, KI) and R-form.
*
* - Depending on the current Workspace Plane orientation,
* use parameters IJ/JK/KI to specify the XY/YZ/ZX offsets.
* At least one of the IJ/JK/KI parameters is required.
* XY/YZ/ZX can be omitted to do a complete circle.
* The given XY/YZ/ZX is not error-checked. The arc ends
* based on the angle of the destination.
* Mixing IJ/JK/KI with R will throw an error.
*
* - R specifies the radius. X or Y (Y or Z / Z or X) is required.
* Omitting both XY/YZ/ZX will throw an error.
* XY/YZ/ZX must differ from the current XY/YZ/ZX.
* Mixing R with IJ/JK/KI will throw an error.
*
* - P specifies the number of full circles to do
* before the specified arc move.
*
* Examples:
*
* G2 I10 ; CW circle centered at X+10
* G3 X20 Y12 R14 ; CCW circle with r=14 ending at X20 Y12
*/
2017-09-16 03:57:52 +02:00
void GcodeSuite::G2_G3(const bool clockwise) {
if (MOTION_CONDITIONS) {
#if ENABLED(SF_ARC_FIX)
const bool relative_mode_backup = relative_mode;
relative_mode = true;
#endif
2017-09-16 03:57:52 +02:00
get_destination_from_command();
#if ENABLED(SF_ARC_FIX)
relative_mode = relative_mode_backup;
#endif
2019-09-29 11:25:39 +02:00
ab_float_t arc_offset = { 0, 0 };
if (parser.seenval('R')) {
const float r = parser.value_linear_units();
if (r) {
2019-09-29 11:25:39 +02:00
const xy_pos_t p1 = current_position, p2 = destination;
if (p1 != p2) {
2019-10-11 02:50:08 +02:00
const xy_pos_t d2 = (p2 - p1) * 0.5f; // XY vector to midpoint of move from current
const float e = clockwise ^ (r < 0) ? -1 : 1, // clockwise -1/1, counterclockwise 1/-1
len = d2.magnitude(), // Distance to mid-point of move from current
h2 = (r - len) * (r + len), // factored to reduce rounding error
h = (h2 >= 0) ? SQRT(h2) : 0.0f; // Distance to the arc pivot-point from midpoint
const xy_pos_t s = { -d2.y, d2.x }; // Perpendicular bisector. (Divide by len for unit vector.)
arc_offset = d2 + s / len * e * h; // The calculated offset (mid-point if |r| <= len)
}
}
}
else {
#if ENABLED(CNC_WORKSPACE_PLANES)
char achar, bchar;
switch (gcode.workspace_plane) {
default:
case GcodeSuite::PLANE_XY: achar = 'I'; bchar = 'J'; break;
case GcodeSuite::PLANE_YZ: achar = 'J'; bchar = 'K'; break;
case GcodeSuite::PLANE_ZX: achar = 'K'; bchar = 'I'; break;
}
#else
constexpr char achar = 'I', bchar = 'J';
#endif
if (parser.seenval(achar)) arc_offset.a = parser.value_linear_units();
if (parser.seenval(bchar)) arc_offset.b = parser.value_linear_units();
}
2019-09-29 11:25:39 +02:00
if (arc_offset) {
#if ENABLED(ARC_P_CIRCLES)
// P indicates number of circles to do
int8_t circles_to_do = parser.byteval('P');
if (!WITHIN(circles_to_do, 0, 100))
SERIAL_ERROR_MSG(MSG_ERR_ARC_ARGS);
while (circles_to_do--)
plan_arc(current_position, arc_offset, clockwise);
#endif
// Send the arc to the planner
plan_arc(destination, arc_offset, clockwise);
reset_stepper_timeout();
}
else
SERIAL_ERROR_MSG(MSG_ERR_ARC_ARGS);
}
}
2017-09-16 03:57:52 +02:00
#endif // ARC_SUPPORT