|
|
|
|
@ -54,6 +54,9 @@
|
|
|
|
|
* G10 - Retract filament according to settings of M207
|
|
|
|
|
* G11 - Retract recover filament according to settings of M208
|
|
|
|
|
* G12 - Clean tool
|
|
|
|
|
* G17 - Select Plane XY (Requires CNC_WORKSPACE_PLANES)
|
|
|
|
|
* G18 - Select Plane ZX (Requires CNC_WORKSPACE_PLANES)
|
|
|
|
|
* G19 - Select Plane YZ (Requires CNC_WORKSPACE_PLANES)
|
|
|
|
|
* G20 - Set input units to inches
|
|
|
|
|
* G21 - Set input units to millimeters
|
|
|
|
|
* G26 - Mesh Validation Pattern (Requires UBL_G26_MESH_VALIDATION)
|
|
|
|
|
@ -688,6 +691,10 @@ static bool send_ok[BUFSIZE];
|
|
|
|
|
millis_t lastUpdateMillis;
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if ENABLED(CNC_WORKSPACE_PLANES)
|
|
|
|
|
static WorkspacePlane workspace_plane = PLANE_XY;
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
FORCE_INLINE float pgm_read_any(const float *p) { return pgm_read_float_near(p); }
|
|
|
|
|
FORCE_INLINE signed char pgm_read_any(const signed char *p) { return pgm_read_byte_near(p); }
|
|
|
|
|
|
|
|
|
|
@ -3264,6 +3271,9 @@ inline void gcode_G0_G1(
|
|
|
|
|
* X or Y must differ from the current XY.
|
|
|
|
|
* Mixing R with I or J 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
|
|
|
|
|
@ -3288,27 +3298,39 @@ inline void gcode_G0_G1(
|
|
|
|
|
float arc_offset[2] = { 0.0, 0.0 };
|
|
|
|
|
if (parser.seen('R')) {
|
|
|
|
|
const float r = parser.value_linear_units(),
|
|
|
|
|
x1 = current_position[X_AXIS], y1 = current_position[Y_AXIS],
|
|
|
|
|
x2 = destination[X_AXIS], y2 = destination[Y_AXIS];
|
|
|
|
|
if (r && (x2 != x1 || y2 != y1)) {
|
|
|
|
|
p1 = current_position[X_AXIS], q1 = current_position[Y_AXIS],
|
|
|
|
|
p2 = destination[X_AXIS], q2 = destination[Y_AXIS];
|
|
|
|
|
if (r && (p2 != p1 || q2 != q1)) {
|
|
|
|
|
const float e = clockwise ^ (r < 0) ? -1 : 1, // clockwise -1/1, counterclockwise 1/-1
|
|
|
|
|
dx = x2 - x1, dy = y2 - y1, // X and Y differences
|
|
|
|
|
dx = p2 - p1, dy = q2 - q1, // X and Y differences
|
|
|
|
|
d = HYPOT(dx, dy), // Linear distance between the points
|
|
|
|
|
h = SQRT(sq(r) - sq(d * 0.5)), // Distance to the arc pivot-point
|
|
|
|
|
mx = (x1 + x2) * 0.5, my = (y1 + y2) * 0.5, // Point between the two points
|
|
|
|
|
mx = (p1 + p2) * 0.5, my = (q1 + q2) * 0.5, // Point between the two points
|
|
|
|
|
sx = -dy / d, sy = dx / d, // Slope of the perpendicular bisector
|
|
|
|
|
cx = mx + e * h * sx, cy = my + e * h * sy; // Pivot-point of the arc
|
|
|
|
|
arc_offset[X_AXIS] = cx - x1;
|
|
|
|
|
arc_offset[Y_AXIS] = cy - y1;
|
|
|
|
|
arc_offset[0] = cx - p1;
|
|
|
|
|
arc_offset[1] = cy - q1;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
else {
|
|
|
|
|
if (parser.seen('I')) arc_offset[X_AXIS] = parser.value_linear_units();
|
|
|
|
|
if (parser.seen('J')) arc_offset[Y_AXIS] = parser.value_linear_units();
|
|
|
|
|
if (parser.seen('I')) arc_offset[0] = parser.value_linear_units();
|
|
|
|
|
if (parser.seen('J')) arc_offset[1] = parser.value_linear_units();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (arc_offset[0] || arc_offset[1]) {
|
|
|
|
|
// Send an arc to the planner
|
|
|
|
|
|
|
|
|
|
#if ENABLED(ARC_P_CIRCLES)
|
|
|
|
|
// P indicates number of circles to do
|
|
|
|
|
int8_t circles_to_do = parser.seen('P') ? parser.value_byte() : 0;
|
|
|
|
|
if (!WITHIN(circles_to_do, 0, 100)) {
|
|
|
|
|
SERIAL_ERROR_START();
|
|
|
|
|
SERIAL_ERRORLNPGM(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);
|
|
|
|
|
refresh_cmd_timeout();
|
|
|
|
|
}
|
|
|
|
|
@ -3408,6 +3430,25 @@ inline void gcode_G4() {
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if ENABLED(CNC_WORKSPACE_PLANES)
|
|
|
|
|
|
|
|
|
|
void report_workspace_plane() {
|
|
|
|
|
SERIAL_ECHO_START();
|
|
|
|
|
SERIAL_ECHOPGM("Workspace Plane ");
|
|
|
|
|
serialprintPGM(workspace_plane == PLANE_YZ ? PSTR("YZ\n") : workspace_plane == PLANE_ZX ? PSTR("ZX\n") : PSTR("XY\n"));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* G17: Select Plane XY
|
|
|
|
|
* G18: Select Plane ZX
|
|
|
|
|
* G19: Select Plane YZ
|
|
|
|
|
*/
|
|
|
|
|
inline void gcode_G17() { workspace_plane = PLANE_XY; }
|
|
|
|
|
inline void gcode_G18() { workspace_plane = PLANE_ZX; }
|
|
|
|
|
inline void gcode_G19() { workspace_plane = PLANE_YZ; }
|
|
|
|
|
|
|
|
|
|
#endif // CNC_WORKSPACE_PLANES
|
|
|
|
|
|
|
|
|
|
#if ENABLED(INCH_MODE_SUPPORT)
|
|
|
|
|
/**
|
|
|
|
|
* G20: Set input mode to inches
|
|
|
|
|
@ -3722,6 +3763,10 @@ inline void gcode_G28(const bool always_home_all) {
|
|
|
|
|
set_bed_leveling_enabled(false);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if ENABLED(CNC_WORKSPACE_PLANES)
|
|
|
|
|
workspace_plane = PLANE_XY;
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
// Always home with tool 0 active
|
|
|
|
|
#if HOTENDS > 1
|
|
|
|
|
const uint8_t old_tool_index = active_extruder;
|
|
|
|
|
@ -10311,6 +10356,18 @@ void process_next_command() {
|
|
|
|
|
break;
|
|
|
|
|
#endif // NOZZLE_CLEAN_FEATURE
|
|
|
|
|
|
|
|
|
|
#if ENABLED(CNC_WORKSPACE_PLANES)
|
|
|
|
|
case 17: // G17: Select Plane XY
|
|
|
|
|
gcode_G17();
|
|
|
|
|
break;
|
|
|
|
|
case 18: // G18: Select Plane ZX
|
|
|
|
|
gcode_G18();
|
|
|
|
|
break;
|
|
|
|
|
case 19: // G19: Select Plane YZ
|
|
|
|
|
gcode_G19();
|
|
|
|
|
break;
|
|
|
|
|
#endif // CNC_WORKSPACE_PLANES
|
|
|
|
|
|
|
|
|
|
#if ENABLED(INCH_MODE_SUPPORT)
|
|
|
|
|
case 20: //G20: Inch Mode
|
|
|
|
|
gcode_G20();
|
|
|
|
|
@ -11922,6 +11979,12 @@ void prepare_move_to_destination() {
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#if ENABLED(ARC_SUPPORT)
|
|
|
|
|
|
|
|
|
|
#if N_ARC_CORRECTION < 1
|
|
|
|
|
#undef N_ARC_CORRECTION
|
|
|
|
|
#define N_ARC_CORRECTION 1
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* Plan an arc in 2 dimensions
|
|
|
|
|
*
|
|
|
|
|
@ -11936,26 +11999,36 @@ void prepare_move_to_destination() {
|
|
|
|
|
float *offset, // Center of rotation relative to current_position
|
|
|
|
|
uint8_t clockwise // Clockwise?
|
|
|
|
|
) {
|
|
|
|
|
#if ENABLED(CNC_WORKSPACE_PLANES)
|
|
|
|
|
AxisEnum p_axis, q_axis, l_axis;
|
|
|
|
|
switch (workspace_plane) {
|
|
|
|
|
case PLANE_XY: p_axis = X_AXIS; q_axis = Y_AXIS; l_axis = Z_AXIS; break;
|
|
|
|
|
case PLANE_ZX: p_axis = Z_AXIS; q_axis = X_AXIS; l_axis = Y_AXIS; break;
|
|
|
|
|
case PLANE_YZ: p_axis = Y_AXIS; q_axis = Z_AXIS; l_axis = X_AXIS; break;
|
|
|
|
|
}
|
|
|
|
|
#else
|
|
|
|
|
constexpr AxisEnum p_axis = X_AXIS, q_axis = Y_AXIS, l_axis = Z_AXIS;
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
float r_X = -offset[X_AXIS], // Radius vector from center to current location
|
|
|
|
|
r_Y = -offset[Y_AXIS];
|
|
|
|
|
// Radius vector from center to current location
|
|
|
|
|
float r_P = -offset[0], r_Q = -offset[1];
|
|
|
|
|
|
|
|
|
|
const float radius = HYPOT(r_X, r_Y),
|
|
|
|
|
center_X = current_position[X_AXIS] - r_X,
|
|
|
|
|
center_Y = current_position[Y_AXIS] - r_Y,
|
|
|
|
|
rt_X = logical[X_AXIS] - center_X,
|
|
|
|
|
rt_Y = logical[Y_AXIS] - center_Y,
|
|
|
|
|
linear_travel = logical[Z_AXIS] - current_position[Z_AXIS],
|
|
|
|
|
const float radius = HYPOT(r_P, r_Q),
|
|
|
|
|
center_P = current_position[p_axis] - r_P,
|
|
|
|
|
center_Q = current_position[q_axis] - r_Q,
|
|
|
|
|
rt_X = logical[p_axis] - center_P,
|
|
|
|
|
rt_Y = logical[q_axis] - center_Q,
|
|
|
|
|
linear_travel = logical[l_axis] - current_position[l_axis],
|
|
|
|
|
extruder_travel = logical[E_AXIS] - current_position[E_AXIS];
|
|
|
|
|
|
|
|
|
|
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
|
|
|
|
|
float angular_travel = ATAN2(r_X * rt_Y - r_Y * rt_X, r_X * rt_X + r_Y * rt_Y);
|
|
|
|
|
float angular_travel = ATAN2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y);
|
|
|
|
|
if (angular_travel < 0) angular_travel += RADIANS(360);
|
|
|
|
|
if (clockwise) angular_travel -= RADIANS(360);
|
|
|
|
|
|
|
|
|
|
// Make a circle if the angular rotation is 0
|
|
|
|
|
if (angular_travel == 0 && current_position[X_AXIS] == logical[X_AXIS] && current_position[Y_AXIS] == logical[Y_AXIS])
|
|
|
|
|
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] == logical[p_axis] && current_position[q_axis] == logical[q_axis])
|
|
|
|
|
angular_travel = RADIANS(360);
|
|
|
|
|
|
|
|
|
|
const float mm_of_travel = HYPOT(angular_travel * radius, FABS(linear_travel));
|
|
|
|
|
if (mm_of_travel < 0.001) return;
|
|
|
|
|
@ -11998,7 +12071,7 @@ void prepare_move_to_destination() {
|
|
|
|
|
cos_T = 1 - 0.5 * sq(theta_per_segment); // Small angle approximation
|
|
|
|
|
|
|
|
|
|
// Initialize the linear axis
|
|
|
|
|
arc_target[Z_AXIS] = current_position[Z_AXIS];
|
|
|
|
|
arc_target[l_axis] = current_position[l_axis];
|
|
|
|
|
|
|
|
|
|
// Initialize the extruder axis
|
|
|
|
|
arc_target[E_AXIS] = current_position[E_AXIS];
|
|
|
|
|
@ -12007,7 +12080,10 @@ void prepare_move_to_destination() {
|
|
|
|
|
|
|
|
|
|
millis_t next_idle_ms = millis() + 200UL;
|
|
|
|
|
|
|
|
|
|
int8_t count = 0;
|
|
|
|
|
#if N_ARC_CORRECTION > 1
|
|
|
|
|
int8_t count = N_ARC_CORRECTION;
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
|
|
|
|
|
|
|
|
|
|
thermalManager.manage_heater();
|
|
|
|
|
@ -12016,28 +12092,33 @@ void prepare_move_to_destination() {
|
|
|
|
|
idle();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (++count < N_ARC_CORRECTION) {
|
|
|
|
|
// Apply vector rotation matrix to previous r_X / 1
|
|
|
|
|
const float r_new_Y = r_X * sin_T + r_Y * cos_T;
|
|
|
|
|
r_X = r_X * cos_T - r_Y * sin_T;
|
|
|
|
|
r_Y = r_new_Y;
|
|
|
|
|
}
|
|
|
|
|
else {
|
|
|
|
|
#if N_ARC_CORRECTION > 1
|
|
|
|
|
if (--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;
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
#endif
|
|
|
|
|
{
|
|
|
|
|
#if N_ARC_CORRECTION > 1
|
|
|
|
|
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);
|
|
|
|
|
r_X = -offset[X_AXIS] * cos_Ti + offset[Y_AXIS] * sin_Ti;
|
|
|
|
|
r_Y = -offset[X_AXIS] * sin_Ti - offset[Y_AXIS] * cos_Ti;
|
|
|
|
|
count = 0;
|
|
|
|
|
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;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Update arc_target location
|
|
|
|
|
arc_target[X_AXIS] = center_X + r_X;
|
|
|
|
|
arc_target[Y_AXIS] = center_Y + r_Y;
|
|
|
|
|
arc_target[Z_AXIS] += linear_per_segment;
|
|
|
|
|
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;
|
|
|
|
|
|
|
|
|
|
clamp_to_software_endstops(arc_target);
|
|
|
|
|
|
Scott Lahteine
7 years ago