Add support for G2/G3 with R parameter

This commit is contained in:
Scott Lahteine 2016-09-18 20:23:01 -05:00
parent ff38a3c3b1
commit c2744d8a8b
2 changed files with 51 additions and 9 deletions

View File

@ -2511,6 +2511,25 @@ inline void gcode_G0_G1() {
/**
* G2: Clockwise Arc
* G3: Counterclockwise Arc
*
* This command has two forms: IJ-form and R-form.
*
* - I specifies an X offset. J specifies a Y offset.
* At least one of the IJ parameters is required.
* X and Y can be omitted to do a complete circle.
* The given XY is not error-checked. The arc ends
* based on the angle of the destination.
* Mixing I or J with R will throw an error.
*
* - R specifies the radius. X or Y is required.
* Omitting both X and Y will throw an error.
* X or Y must differ from the current XY.
* Mixing R with I or J will throw an error.
*
* Examples:
*
* G2 I10 ; CW circle centered at X+10
* G3 X20 Y12 R14 ; CCW circle with r=14 ending at X20 Y12
*/
#if ENABLED(ARC_SUPPORT)
inline void gcode_G2_G3(bool clockwise) {
@ -2527,16 +2546,38 @@ inline void gcode_G0_G1() {
relative_mode = relative_mode_backup;
#endif
// Center of arc as offset from current_position
float arc_offset[2] = {
code_seen('I') ? code_value_axis_units(X_AXIS) : 0,
code_seen('J') ? code_value_axis_units(Y_AXIS) : 0
};
float arc_offset[2] = { 0.0, 0.0 };
if (code_seen('R')) {
const float r = code_value_axis_units(X_AXIS),
x1 = current_position[X_AXIS], y1 = current_position[Y_AXIS],
x2 = destination[X_AXIS], y2 = destination[Y_AXIS];
if (r && (x2 != x1 || y2 != y1)) {
const float e = clockwise ? -1 : 1, // clockwise -1, counterclockwise 1
dx = x2 - x1, dy = y2 - y1, // 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
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;
}
}
else {
if (code_seen('I')) arc_offset[X_AXIS] = code_value_axis_units(X_AXIS);
if (code_seen('J')) arc_offset[Y_AXIS] = code_value_axis_units(Y_AXIS);
}
// Send an arc to the planner
plan_arc(destination, arc_offset, clockwise);
refresh_cmd_timeout();
if (arc_offset[0] || arc_offset[1]) {
// Send an arc to the planner
plan_arc(destination, arc_offset, clockwise);
refresh_cmd_timeout();
}
else {
// Bad arguments
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM(MSG_ERR_ARC_ARGS);
}
}
}
#endif

View File

@ -150,6 +150,7 @@
#define MSG_ERR_MATERIAL_INDEX "M145 S<index> out of range (0-1)"
#define MSG_ERR_M421_PARAMETERS "M421 requires XYZ or IJZ parameters"
#define MSG_ERR_MESH_XY "Mesh XY or IJ cannot be resolved"
#define MSG_ERR_ARC_ARGS "G2/G3 bad parameters"
#define MSG_ERR_M428_TOO_FAR "Too far from reference point"
#define MSG_ERR_M303_DISABLED "PIDTEMP disabled"
#define MSG_M119_REPORT "Reporting endstop status"