bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Add custom types for position (#15204)

Browse Source
This commit is contained in:
Scott Lahteine
2019-09-29 04:25:39 -05:00
committed by GitHub
parent 43d6e9fa43
commit 50e4545255
227 changed files with 3147 additions and 3264 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -50,9 +50,9 @@
* options for G2/G3 arc generation. In future these options may be GCode tunable.
*/
void plan_arc(
const float (&cart)[XYZE], // Destination position
const float (&offset)[2], // Center of rotation relative to current_position
const uint8_t clockwise // Clockwise?
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;
@@ -67,21 +67,21 @@ void plan_arc(
#endif
// Radius vector from center to current location
float r_P = -offset[0], r_Q = -offset[1];
ab_float_t rvec = -offset;
const float radius = HYPOT(r_P, r_Q),
const float radius = HYPOT(rvec.a, rvec.b),
#if ENABLED(AUTO_BED_LEVELING_UBL)
start_L = current_position[l_axis],
#endif
center_P = current_position[p_axis] - r_P,
center_Q = current_position[q_axis] - r_Q,
center_P = current_position[p_axis] - rvec.a,
center_Q = current_position[q_axis] - rvec.b,
rt_X = cart[p_axis] - center_P,
rt_Y = cart[q_axis] - center_Q,
linear_travel = cart[l_axis] - current_position[l_axis],
extruder_travel = cart[E_AXIS] - current_position[E_AXIS];
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.
float angular_travel = ATAN2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y);
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)));
@@ -133,7 +133,7 @@ void plan_arc(
* This is important when there are successive arc motions.
*/
// Vector rotation matrix values
float raw[XYZE];
xyze_pos_t raw;
const float theta_per_segment = angular_travel / segments,
linear_per_segment = linear_travel / segments,
extruder_per_segment = extruder_travel / segments,
@@ -144,7 +144,7 @@ void plan_arc(
raw[l_axis] = current_position[l_axis];
// Initialize the extruder axis
raw[E_AXIS] = current_position[E_AXIS];
raw.e = current_position.e;
const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
@@ -168,10 +168,10 @@ void plan_arc(
#if N_ARC_CORRECTION > 1
if (--arc_recalc_count) {
// Apply vector rotation matrix to previous r_P / 1
const float r_new_Y = r_P * sin_T + r_Q * cos_T;
r_P = r_P * cos_T - r_Q * sin_T;
r_Q = r_new_Y;
// 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
@@ -185,20 +185,20 @@ void plan_arc(
// 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);
r_P = -offset[0] * cos_Ti + offset[1] * sin_Ti;
r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti;
rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti;
rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti;
}
// Update raw location
raw[p_axis] = center_P + r_P;
raw[q_axis] = center_Q + r_Q;
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;
UNUSED(linear_per_segment);
#else
raw[l_axis] += linear_per_segment;
#endif
raw[E_AXIS] += extruder_per_segment;
raw.e += extruder_per_segment;
apply_motion_limits(raw);
@@ -215,7 +215,7 @@ void plan_arc(
}
// Ensure last segment arrives at target location.
COPY(raw, cart);
raw = cart;
#if ENABLED(AUTO_BED_LEVELING_UBL)
raw[l_axis] = start_L;
#endif
@@ -235,7 +235,7 @@ void plan_arc(
#if ENABLED(AUTO_BED_LEVELING_UBL)
raw[l_axis] = start_L;
#endif
COPY(current_position, raw);
current_position = raw;
} // plan_arc
/**
@@ -278,32 +278,27 @@ void GcodeSuite::G2_G3(const bool clockwise) {
relative_mode = relative_mode_backup;
#endif
float arc_offset[2] = { 0, 0 };
ab_float_t arc_offset = { 0, 0 };
if (parser.seenval('R')) {
const float r = parser.value_linear_units();
if (r) {
const float p1 = current_position[X_AXIS], q1 = current_position[Y_AXIS],
p2 = destination[X_AXIS], q2 = destination[Y_AXIS];
if (p2 != p1 || q2 != q1) {
const float e = clockwise ^ (r < 0) ? -1 : 1, // clockwise -1/1, counterclockwise 1/-1
dx = p2 - p1, dy = q2 - q1, // X and Y differences
d = HYPOT(dx, dy), // Linear distance between the points
dinv = 1/d, // Inverse of d
h = SQRT(sq(r) - sq(d * 0.5f)), // Distance to the arc pivot-point
mx = (p1 + p2) * 0.5f, my = (q1 + q2) * 0.5f,// Point between the two points
sx = -dy * dinv, sy = dx * dinv, // Slope of the perpendicular bisector
cx = mx + e * h * sx, cy = my + e * h * sy; // Pivot-point of the arc
arc_offset[0] = cx - p1;
arc_offset[1] = cy - q1;
const xy_pos_t p1 = current_position, p2 = destination;
if (p1 != p2) {
const xy_pos_t d = p2 - p1, m = (p1 + p2) * 0.5f; // XY distance and midpoint
const float e = clockwise ^ (r < 0) ? -1 : 1, // clockwise -1/1, counterclockwise 1/-1
len = d.magnitude(), // Total move length
h = SQRT(sq(r) - sq(len * 0.5f)); // Distance to the arc pivot-point
const xy_pos_t s = { d.x, -d.y }; // Inverse Slope of the perpendicular bisector
arc_offset = m + s * RECIPROCAL(len) * e * h - p1; // The calculated offset
}
}
}
else {
if (parser.seenval('I')) arc_offset[0] = parser.value_linear_units();
if (parser.seenval('J')) arc_offset[1] = parser.value_linear_units();
if (parser.seenval('I')) arc_offset.a = parser.value_linear_units();
if (parser.seenval('J')) arc_offset.b = parser.value_linear_units();
}
if (arc_offset[0] || arc_offset[1]) {
if (arc_offset) {
#if ENABLED(ARC_P_CIRCLES)
// P indicates number of circles to do