// 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 gcode_execute_line(char *line) {
uint8_t char_counter = 0;
char letter;
double value;
int int_value;
double unit_converted_value;
uint8_t next_action = NEXT_ACTION_NONE;
double target[3];
double p = 0.0;
int cs = 0;
int l = 0;
bool got_actual_line_command = false; // as opposed to just e.g. G1 F1200
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 = next_action = NEXT_ACTION_SEEK; break;
case 1: gc.motion_mode = next_action = NEXT_ACTION_FEED; break;
case 4: next_action = NEXT_ACTION_DWELL; break;
case 10: next_action = NEXT_ACTION_SET_COORDINATE_OFFSET; break;
case 20: gc.inches_mode = true; break;
case 21: gc.inches_mode = false; break;
case 30: next_action = NEXT_ACTION_HOMING_CYCLE; break;
case 54: gc.offselect = OFFSET_G54; break;
case 55: gc.offselect = OFFSET_G55; break;
case 90: gc.absolute_mode = true; break;
case 91: gc.absolute_mode = false; break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'M':
switch(int_value) {
case 7: next_action = NEXT_ACTION_AIR_ENABLE;break;
case 8: next_action = NEXT_ACTION_GAS_ENABLE;break;
case 9: next_action = NEXT_ACTION_AIRGAS_DISABLE;break;
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
}
if (gc.status_code) { break; }
}
// bail when errors
if (gc.status_code) { return gc.status_code; }
char_counter = 0;
memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position
//// Pass 2: Parameters
while(next_statement(&letter, &value, line, &char_counter)) {
if (gc.inches_mode) {
unit_converted_value = value * MM_PER_INCH;
} else {
unit_converted_value = value;
}
switch(letter) {
case 'F':
if (unit_converted_value <= 0) { FAIL(STATUS_BAD_NUMBER_FORMAT); }
if (gc.motion_mode == NEXT_ACTION_SEEK) {
gc.seek_rate = unit_converted_value;
} else {
gc.feed_rate = unit_converted_value;
}
break;
case 'X': case 'Y': case 'Z':
if (gc.absolute_mode) {
target[letter - 'X'] = unit_converted_value;
} else {
target[letter - 'X'] += unit_converted_value;
}
got_actual_line_command = true;
break;
case 'P': // dwelling seconds or CS selector
if (next_action == NEXT_ACTION_SET_COORDINATE_OFFSET) {
cs = trunc(value);
} else {
p = value;
}
break;
case 'S':
gc.nominal_laser_intensity = value;
break;
case 'L': // G10 qualifier
l = trunc(value);
break;
}
}
// bail when error
if (gc.status_code) { return(gc.status_code); }
//// Perform any physical actions
switch (next_action) {
case NEXT_ACTION_SEEK:
if (got_actual_line_command) {
planner_line( target[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS],
target[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS],
target[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS],
gc.seek_rate, 0 );
}
break;
case NEXT_ACTION_FEED:
if (got_actual_line_command) {
planner_line( target[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS],
target[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS],
target[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS],
gc.feed_rate, gc.nominal_laser_intensity );
}
break;
case NEXT_ACTION_DWELL:
planner_dwell(p, gc.nominal_laser_intensity);
break;
// case NEXT_ACTION_STOP:
// planner_stop(); // stop and cancel the remaining program
// gc.position[X_AXIS] = stepper_get_position_x();
// gc.position[Y_AXIS] = stepper_get_position_y();
// gc.position[Z_AXIS] = stepper_get_position_z();
// planner_set_position(gc.position[X_AXIS], gc.position[Y_AXIS], gc.position[Z_AXIS]);
// // move to table origin
// target[X_AXIS] = 0;
// target[Y_AXIS] = 0;
// target[Z_AXIS] = 0;
// planner_line( target[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS],
// target[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS],
// target[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS],
// gc.seek_rate, 0 );
// break;
case NEXT_ACTION_HOMING_CYCLE:
stepper_homing_cycle();
// now that we are at the physical home
// zero all the position vectors
clear_vector(gc.position);
clear_vector(target);
planner_set_position(0.0, 0.0, 0.0);
// move head to g54 offset
gc.offselect = OFFSET_G54;
target[X_AXIS] = 0;
target[Y_AXIS] = 0;
target[Z_AXIS] = 0;
planner_line( target[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS],
target[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS],
target[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS],
gc.seek_rate, 0 );
break;
case NEXT_ACTION_SET_COORDINATE_OFFSET:
if (cs == OFFSET_G54 || cs == OFFSET_G55) {
if (l == 2) {
//set offset to target, eg: G10 L2 P1 X15 Y15 Z0
gc.offsets[3*cs+X_AXIS] = target[X_AXIS];
gc.offsets[3*cs+Y_AXIS] = target[Y_AXIS];
gc.offsets[3*cs+Z_AXIS] = target[Z_AXIS];
// Set target in ref to new coord system so subsequent moves are calculated correctly.
target[X_AXIS] = (gc.position[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS]) - gc.offsets[3*cs+X_AXIS];
target[Y_AXIS] = (gc.position[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS]) - gc.offsets[3*cs+Y_AXIS];
target[Z_AXIS] = (gc.position[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS]) - gc.offsets[3*cs+Z_AXIS];
} else if (l == 20) {
// set offset to current pos, eg: G10 L20 P2
gc.offsets[3*cs+X_AXIS] = gc.position[X_AXIS] + gc.offsets[3*gc.offselect+X_AXIS];
gc.offsets[3*cs+Y_AXIS] = gc.position[Y_AXIS] + gc.offsets[3*gc.offselect+Y_AXIS];
gc.offsets[3*cs+Z_AXIS] = gc.position[Z_AXIS] + gc.offsets[3*gc.offselect+Z_AXIS];
target[X_AXIS] = 0;
target[Y_AXIS] = 0;
target[Z_AXIS] = 0;
}
}
break;
case NEXT_ACTION_AIRGAS_DISABLE:
planner_control_airgas_disable();
break;
case NEXT_ACTION_AIR_ENABLE:
planner_control_air_enable();
break;
case NEXT_ACTION_GAS_ENABLE:
planner_control_gas_enable();
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;
}
// The arc is approximated by generating a huge number of tiny, linear segments. The length of each
// segment is configured in settings.mm_per_arc_segment.
void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1,
uint8_t axis_linear, double feed_rate, double radius, uint8_t isclockwise, double acceleration,
uint8_t laser_pwm, uint16_t laser_ppi)
{
// int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
// plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
double center_axis0 = position[axis_0] + offset[axis_0];
double center_axis1 = position[axis_1] + offset[axis_1];
double linear_travel = target[axis_linear] - position[axis_linear];
double r_axis0 = -offset[axis_0]; // Radius vector from center to current location
double r_axis1 = -offset[axis_1];
double rt_axis0 = target[axis_0] - center_axis0;
double rt_axis1 = target[axis_1] - center_axis1;
// CCW angle between position and target from circle center. Only one atan2() trig computation required.
double angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
if (angular_travel < 0) { angular_travel += 2*M_PI; }
if (isclockwise) { angular_travel -= 2*M_PI; }
double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
if (millimeters_of_travel < 0.001) { return; }
uint16_t segments = floor(millimeters_of_travel/MM_PER_ARC_SEGMENT);
if(segments == 0) segments = 1;
/*
// Multiply inverse feed_rate to compensate for the fact that this movement is approximated
// by a number of discrete segments. The inverse feed_rate should be correct for the sum of
// all segments.
if (invert_feed_rate) { feed_rate *= segments; }
*/
double theta_per_segment = angular_travel/segments;
double linear_per_segment = linear_travel/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 mc_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 mc_arc overhead.
This is important when there are successive arc motions.
*/
// Vector rotation matrix values
double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
double sin_T = theta_per_segment;
double arc_target[4];
double sin_Ti;
double cos_Ti;
double r_axisi;
uint16_t i;
int8_t count = 0;
// Initialize the linear axis
arc_target[axis_linear] = position[axis_linear];
for (i = 1; i<segments; i++) { // Increment (segments-1)
if (count < N_ARC_CORRECTION) {
// Apply vector rotation matrix
r_axisi = r_axis0*sin_T + r_axis1*cos_T;
r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
r_axis1 = r_axisi;
count++;
} else {
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
cos_Ti = cos(i*theta_per_segment);
sin_Ti = sin(i*theta_per_segment);
r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
count = 0;
}
// Update arc_target location
arc_target[axis_0] = center_axis0 + r_axis0;
arc_target[axis_1] = center_axis1 + r_axis1;
arc_target[axis_linear] += linear_per_segment;
planner_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS],
feed_rate, acceleration,
laser_pwm, laser_ppi);
}
// Ensure last segment arrives at target location.
planner_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS],
feed_rate, acceleration,
laser_pwm, laser_ppi);
// plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
}
