// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace).
uint8_t gc_execute_line(char *line) {
uint8_t char_counter = 0;
char letter;
double value;
double unit_converted_value;
double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
int radius_mode = false;
uint8_t absolute_override = false; /* 1 = absolute motion for this block only {G53} */
uint8_t next_action = NEXT_ACTION_DEFAULT; /* The action that will be taken by the parsed line */
double target[3], offset[3];
double p = 0, r = 0;
int int_value;
clear_vector(target);
clear_vector(offset);
gc.status_code = STATUS_OK;
// Disregard comments and block delete
if (line[0] == '(') { return(gc.status_code); }
if (line[0] == '/') { char_counter++; } // ignore block delete
// Pass 1: Commands
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
switch(letter) {
case 'G':
switch(int_value) {
case 0: gc.motion_mode = MOTION_MODE_SEEK; break;
case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
#ifdef __AVR_ATmega328P__
case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
#endif
case 4: next_action = NEXT_ACTION_DWELL; break;
case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break;
case 53: absolute_override = true; break;
case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
case 92: next_action = NEXT_ACTION_SET_COORDINATE_OFFSET; break;
case 93: gc.inverse_feed_rate_mode = true; break;
case 94: gc.inverse_feed_rate_mode = false; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
switch(int_value) {
case 0: case 1: gc.program_flow = PROGRAM_FLOW_PAUSED; break;
case 2: case 30: case 60: gc.program_flow = PROGRAM_FLOW_COMPLETED; break;
case 3: gc.spindle_direction = 1; break;
case 4: gc.spindle_direction = -1; break;
case 5: gc.spindle_direction = 0; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'T': gc.tool = trunc(value); break;
}
if(gc.status_code) { break; }
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
char_counter = 0;
clear_vector(offset);
memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position
// Pass 2: Parameters
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
unit_converted_value = to_millimeters(value);
switch(letter) {
case 'F':
if (gc.inverse_feed_rate_mode) {
inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only
} else {
if (gc.motion_mode == MOTION_MODE_SEEK) {
gc.seek_rate = unit_converted_value/60;
} else {
gc.feed_rate = unit_converted_value/60; // millimeters pr second
}
}
break;
case 'I': case 'J': case 'K': offset[letter-'I'] = unit_converted_value; break;
case 'P': p = value; break;
case 'R': r = unit_converted_value; radius_mode = true; break;
case 'S': gc.spindle_speed = value; break;
case 'X': case 'Y': case 'Z':
if (gc.absolute_mode || absolute_override) {
target[letter - 'X'] = unit_converted_value;
} else {
target[letter - 'X'] += unit_converted_value;
}
break;
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
// Update spindle state
spindle_run(gc.spindle_direction, gc.spindle_speed);
// Perform any physical actions
switch (next_action) {
case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(gc.position); break; // Go home
case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break; // Dwell
case NEXT_ACTION_SET_COORDINATE_OFFSET: // Coordinate System offset
mc_set_current_position(target[X_AXIS], target[Y_AXIS], target[Z_AXIS]);
break;
case NEXT_ACTION_DEFAULT:
switch (gc.motion_mode) {
case MOTION_MODE_CANCEL: break;
case MOTION_MODE_SEEK:
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false);
break;
case MOTION_MODE_LINEAR:
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode);
break;
#ifdef __AVR_ATmega328P__
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
if (radius_mode) {
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
double x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
double y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(offset);
double h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d)
// If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any
// real CNC, and thus - for practical reasons - we will terminate promptly:
if(isnan(h_x2_div_d)) { FAIL(STATUS_FLOATING_POINT_ERROR); return(gc.status_code); }
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T <-- Target position
^
Clockwise circles with this center | Clockwise circles with this center will have
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
|
C <-- Current position */
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
if (r < 0) { h_x2_div_d = -h_x2_div_d; }
// Complete the operation by calculating the actual center of the arc
offset[gc.plane_axis_0] = (x-(y*h_x2_div_d))/2;
offset[gc.plane_axis_1] = (y+(x*h_x2_div_d))/2;
}
/*
This segment sets up an clockwise or counterclockwise arc from the current position to the target position around
the center designated by the offset vector. All theta-values measured in radians of deviance from the positive
y-axis.
| <- theta == 0
* * *
* *
* *
* O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2)
* /
C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
*/
// calculate the theta (angle) of the current point
double theta_start = theta(-offset[gc.plane_axis_0], -offset[gc.plane_axis_1]);
// calculate the theta (angle) of the target point
double theta_end = theta(target[gc.plane_axis_0] - offset[gc.plane_axis_0] - gc.position[gc.plane_axis_0],
target[gc.plane_axis_1] - offset[gc.plane_axis_1] - gc.position[gc.plane_axis_1]);
// ensure that the difference is positive so that we have clockwise travel
if (theta_end < theta_start) { theta_end += 2*M_PI; }
double angular_travel = theta_end-theta_start;
// Invert angular motion if the g-code wanted a counterclockwise arc
if (gc.motion_mode == MOTION_MODE_CCW_ARC) {
angular_travel = angular_travel-2*M_PI;
}
// Find the radius
double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]);
// Calculate the motion along the depth axis of the helix
double depth = target[gc.plane_axis_2]-gc.position[gc.plane_axis_2];
// Trace the arc
mc_arc(theta_start, angular_travel, radius, depth, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
gc.position);
// Finish off with a line to make sure we arrive exactly where we think we are
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode);
break;
#endif
}
}
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(gc.position, target, sizeof(double)*3); // gc.position[] = target[];
return(gc.status_code);
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace). Comments and block delete
// characters have been removed.
uint8_t gc_execute_line(char *line) {
uint8_t char_counter = 0;
char letter;
double value;
double unit_converted_value;
double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
uint8_t radius_mode = false;
uint8_t absolute_override = false; /* 1 = absolute motion for this block only {G53} */
uint8_t next_action = NEXT_ACTION_DEFAULT; /* The action that will be taken by the parsed line */
double target[3], offset[3];
double p = 0, r = 0;
int int_value;
gc.status_code = STATUS_OK;
// Pass 1: Commands
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
switch(letter) {
case 'G':
switch(int_value) {
case 0: gc.motion_mode = MOTION_MODE_SEEK; break;
case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
case 4: next_action = NEXT_ACTION_DWELL; break;
case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break;
case 53: absolute_override = true; break;
case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
case 92: next_action = NEXT_ACTION_SET_COORDINATE_OFFSET; break;
case 93: gc.inverse_feed_rate_mode = true; break;
case 94: gc.inverse_feed_rate_mode = false; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
switch(int_value) {
case 0: case 1: gc.program_flow = PROGRAM_FLOW_PAUSED; break;
case 2: case 30: case 60: gc.program_flow = PROGRAM_FLOW_COMPLETED; break;
case 3: gc.laser_enable = 1; break;
case 4: gc.laser_enable = 1; break;
case 5: gc.laser_enable = 0; break;
case 112: next_action = NEXT_ACTION_CANCEL; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'T': gc.tool = trunc(value); break;
}
if(gc.status_code) { break; }
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
char_counter = 0;
clear_vector(target);
clear_vector(offset);
memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position
// Pass 2: Parameters
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
unit_converted_value = to_millimeters(value);
switch(letter) {
case 'F':
if (unit_converted_value <= 0) { FAIL(STATUS_BAD_NUMBER_FORMAT); } // Must be greater than zero
if (gc.inverse_feed_rate_mode) {
inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only
} else {
if (gc.motion_mode == MOTION_MODE_SEEK) {
gc.seek_rate = unit_converted_value;
} else {
gc.feed_rate = unit_converted_value; // millimeters per minute
}
}
break;
case 'I': case 'J': case 'K': offset[letter-'I'] = unit_converted_value; break;
case 'P': p = value; break;
case 'R': r = unit_converted_value; radius_mode = true; break;
case 'S': gc.nominal_laser_intensity = value; break;
case 'X': case 'Y': case 'Z':
if (gc.absolute_mode || absolute_override) {
target[letter - 'X'] = unit_converted_value;
} else {
target[letter - 'X'] += unit_converted_value;
}
break;
}
}
// If there were any errors parsing this line, we will return right away with the bad news
if (gc.status_code) { return(gc.status_code); }
// Perform any physical actions
switch (next_action) {
case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(gc.position); break;
case NEXT_ACTION_DWELL: mc_dwell(p); break;
case NEXT_ACTION_SET_COORDINATE_OFFSET:
mc_set_current_position(target[X_AXIS], target[Y_AXIS], target[Z_AXIS]);
break;
case NEXT_ACTION_CANCEL:
//mc_emergency_stop();
// captain, we have a new target!
//st_get_position(&gc.position[X_AXIS], &gc.position[Y_AXIS], &gc.position[Z_AXIS]);
//mc_set_current_position(gc.position[X_AXIS], gc.position[Y_AXIS], gc.position[Z_AXIS]);
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false, LASER_OFF);
//return; // totally bail
break;
case NEXT_ACTION_DEFAULT:
switch (gc.motion_mode) {
case MOTION_MODE_CANCEL: break;
case MOTION_MODE_SEEK:
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false, LASER_OFF);
break;
case MOTION_MODE_LINEAR:
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS],
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
gc.nominal_laser_intensity);
break;
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
if (radius_mode) {
/*
We need to calculate the center of the circle that has the designated radius and passes
through both the current position and the target position. This method calculates the following
set of equations where [x,y] is the vector from current to target position, d == magnitude of
that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the
length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point
[i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
d^2 == x^2 + y^2
h^2 == r^2 - (d/2)^2
i == x/2 - y/d*h
j == y/2 + x/d*h
O <- [i,j]
- |
r - |
- |
- | h
- |
[0,0] -> C -----------------+--------------- T <- [x,y]
| <------ d/2 ---->|
C - Current position
T - Target position
O - center of circle that pass through both C and T
d - distance from C to T
r - designated radius
h - distance from center of CT to O
Expanding the equations:
d -> sqrt(x^2 + y^2)
h -> sqrt(4 * r^2 - x^2 - y^2)/2
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
Which can be written:
i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
Which we for size and speed reasons optimize to:
h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
i = (x - (y * h_x2_div_d))/2
j = (y + (x * h_x2_div_d))/2
*/
// Calculate the change in position along each selected axis
double x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
double y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
clear_vector(offset);
double h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d)
// If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any
// real CNC, and thus - for practical reasons - we will terminate promptly:
if(isnan(h_x2_div_d)) { FAIL(STATUS_FLOATING_POINT_ERROR); return(gc.status_code); }
// Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
/* The counter clockwise circle lies to the left of the target direction. When offset is positive,
the left hand circle will be generated - when it is negative the right hand circle is generated.
T <-- Target position
^
Clockwise circles with this center | Clockwise circles with this center will have
will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing!
\ | /
center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative
|
|
C <-- Current position */
// Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!),
// even though it is advised against ever generating such circles in a single line of g-code. By
// inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
// travel and thus we get the unadvisably long arcs as prescribed.
if (r < 0) {
h_x2_div_d = -h_x2_div_d;
r = -r; // Finished with r. Set to positive for mc_arc
}
// Complete the operation by calculating the actual center of the arc
offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
} else { // Offset mode specific computations
r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
}
// Set clockwise/counter-clockwise sign for mc_arc computations
uint8_t isclockwise = false;
if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
// Trace the arc
mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
(gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
r, isclockwise, gc.nominal_laser_intensity);
break;
}
}
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
// in any intermediate location.
memcpy(gc.position, target, sizeof(double)*3); // gc.position[] = target[];
return(gc.status_code);
}
