// Set relative coordinate system
static const char *handle_G92(float sub_command,
struct GCodeParser *p, const char *line) {
// It is safe to compare raw float values here, as long as we give float
// literals. They have been parsed from literals as well.
if (sub_command == 92.0f) {
char axis_l;
float value;
const char *remaining_line;
while ((remaining_line = gparse_pair(p, line, &axis_l, &value))) {
const float unit_val = value * p->unit_to_mm_factor;
const enum GCodeParserAxis axis = gcodep_letter2axis(axis_l);
if (axis == GCODE_NUM_AXES)
break; // Possibly start of new command.
// This sets the given value to be the new zero.
p->origin_g92[axis] = p->axes_pos[axis] - unit_val;
line = remaining_line;
}
set_current_origin(p, p->origin_g92);
}
else if (sub_command == 92.1f) {
reset_G92(p);
set_current_origin(p, p->origin_g92);
}
else if (sub_command == 92.2f) {
set_current_origin(p, p->origin_machine); // Later: G54...
}
else if (sub_command == 92.3f) {
set_current_origin(p, p->origin_g92);
}
return line;
}
static const char *handle_z_probe(struct GCodeParser *p, const char *line) {
char letter;
float value;
float feedrate = -1;
float probe_thickness = 0;
const char *remaining_line;
while ((remaining_line = gparse_pair(p, line, &letter, &value))) {
const float unit_value = value * p->unit_to_mm_factor;
if (letter == 'F') feedrate = f_param_to_feedrate(unit_value);
else if (letter == 'Z') probe_thickness = value * p->unit_to_mm_factor;
else break;
line = remaining_line;
}
// Probe for the travel endstop
float probed_pos;
if (p->callbacks.probe_axis(p->callbacks.user_data, feedrate, AXIS_Z,
&probed_pos)) {
p->axes_pos[AXIS_Z] = probed_pos;
// Doing implicit G92 here. Is this what we want ? Later, this might
// be part of tool-offset or something.
p->global_offset_g92[AXIS_Z] = (p->axes_pos[AXIS_Z] - probe_thickness)
- p->current_origin[AXIS_Z];
set_current_origin(p, p->current_origin, p->global_offset_g92);
}
return line;
}
// Set a parameter on a user callback.
// These all have the form foo(void *userdata, float value)
static const char *set_param(struct GCodeParser *p, char param_letter,
void (*value_setter)(void *, float), float factor,
const char *line) {
char letter;
float value;
const char *remaining_line = gparse_pair(p, line, &letter, &value);
if (remaining_line != NULL && letter == param_letter) {
value_setter(p->callbacks.user_data, factor * value);
return remaining_line;
}
return line;
}
// For we just generate an arc by emitting many small steps for
// now. TODO(hzeller): actually generate a curve profile for that.
// With G17, G18, G19, the plane was selected before.
// X, Y, Z: new position (two in the plane, one for the helix.
// I, J, K: offset to center, corresponding to X,Y,Z.
// Only the two in the given plane are relevant.
// F : Optional feedrate.
// P : number of turns. currently ignored.
// R : TODO: implement. Modern systems allow that.
static const char *handle_arc(struct GCodeParser *p,
const char *line,
int is_cw) {
const char *remaining_line;
float target[GCODE_NUM_AXES];
float offset[GCODE_NUM_AXES] = {0};
float feedrate = -1;
float value;
char letter;
int turns = 1;
memcpy(target, p->axes_pos, GCODE_NUM_AXES * sizeof(*target));
while ((remaining_line = gparse_pair(p, line, &letter, &value))) {
const float unit_value = value * p->unit_to_mm_factor;
if (letter == 'X') target[AXIS_X] = abs_axis_pos(p, AXIS_X, unit_value);
else if (letter == 'Y') target[AXIS_Y] = abs_axis_pos(p, AXIS_Y, unit_value);
else if (letter == 'Z') target[AXIS_Z] = abs_axis_pos(p, AXIS_Z, unit_value);
else if (letter == 'I') offset[AXIS_X] = unit_value;
else if (letter == 'J') offset[AXIS_Y] = unit_value;
else if (letter == 'K') offset[AXIS_Z] = unit_value;
else if (letter == 'F') feedrate = f_param_to_feedrate(unit_value);
else if (letter == 'P') turns = (int)value; // currently ignored
// TODO: 'R'
else break;
line = remaining_line;
}
// Should the arc parameters be sanity checked?
if (turns < 0 || turns > 4) {
fprintf(stderr, "G-Code Syntax Error: handle_arc: turns=%d (must be 1-4)\n",
turns);
return remaining_line;
}
struct ArcCallbackData cb_arc_data;
cb_arc_data.parser = p;
cb_arc_data.feedrate = feedrate;
arc_gen(p->arc_normal, is_cw, p->axes_pos, offset, target,
&arc_callback, &cb_arc_data);
return line;
}
static const char *handle_move(struct GCodeParser *p,
const char *line, int force_change) {
char axis_l;
float value;
int any_change = force_change;
float feedrate = -1;
const char *remaining_line;
AxesRegister new_pos;
memcpy(new_pos, p->axes_pos, sizeof(new_pos));
while ((remaining_line = gparse_pair(p, line, &axis_l, &value))) {
const float unit_value = value * p->unit_to_mm_factor;
if (axis_l == 'F') {
feedrate = f_param_to_feedrate(unit_value);
any_change = 1;
}
else {
const enum GCodeParserAxis update_axis = gcodep_letter2axis(axis_l);
if (update_axis == GCODE_NUM_AXES)
break; // Invalid axis: possibly start of new command.
new_pos[update_axis] = abs_axis_pos(p, update_axis, unit_value);
any_change = 1;
}
line = remaining_line;
}
char did_move = 0;
if (any_change) {
if (p->modal_g0_g1) {
did_move = p->callbacks.coordinated_move(p->callbacks.user_data,
feedrate, new_pos);
} else {
did_move = p->callbacks.rapid_move(p->callbacks.user_data,
feedrate, new_pos);
}
}
if (did_move) {
memcpy(p->axes_pos, new_pos, sizeof(p->axes_pos));
}
return line;
}
static const char *handle_home(struct GCodeParser *p, const char *line) {
AxisBitmap_t homing_flags = 0;
char axis_l;
float dummy;
const char *remaining_line;
while ((remaining_line = gparse_pair(p, line, &axis_l, &dummy))) {
const enum GCodeParserAxis axis = gcodep_letter2axis(axis_l);
if (axis == GCODE_NUM_AXES)
break; // Possibly start of new command.
homing_flags |= (1 << axis);
line = remaining_line;
}
if (homing_flags == 0) homing_flags = kAllAxesBitmap;
p->callbacks.go_home(p->callbacks.user_data, homing_flags);
// Now update the world position
for (int i = 0; i < GCODE_NUM_AXES; ++i) {
if (homing_flags & (1 << i)) {
p->axes_pos[i] = p->origin_machine[i];
}
}
return line;
}
// Note: changes here should be documented in G-code.md as well.
void gcodep_parse_line(struct GCodeParser *p, const char *line,
FILE *err_stream) {
++p->line_number;
void *const userdata = p->callbacks.user_data;
struct GCodeParserCb *cb = &p->callbacks;
p->err_msg = err_stream; // remember as 'instance' variable.
char letter;
float value;
while ((line = gparse_pair(p, line, &letter, &value))) {
if (!p->program_in_progress) {
cb->gcode_start(userdata);
p->program_in_progress = 1;
}
char processed_command = 1;
if (letter == 'G') {
switch ((int) value) {
case 0: p->modal_g0_g1 = 0; line = handle_move(p, line, 0); break;
case 1: p->modal_g0_g1 = 1; line = handle_move(p, line, 0); break;
case 2: line = handle_arc(p, line, 1); break;
case 3: line = handle_arc(p, line, 0); break;
case 4: line = set_param(p, 'P', cb->dwell, 1.0f, line); break;
case 17: p->arc_normal = AXIS_Z; break;
case 18: p->arc_normal = AXIS_Y; break;
case 19: p->arc_normal = AXIS_X; break;
case 20: p->unit_to_mm_factor = 25.4f; break;
case 21: p->unit_to_mm_factor = 1.0f; break;
case 28: line = handle_home(p, line); break;
case 30: line = handle_z_probe(p, line); break;
case 70: p->unit_to_mm_factor = 25.4f; break;
case 71: p->unit_to_mm_factor = 1.0f; break;
case 90: set_all_axis_to_absolute(p, 1); break;
case 91: set_all_axis_to_absolute(p, 0); break;
case 92: line = handle_G92(value, p, line); break;
default: line = cb->unprocessed(userdata, letter, value, line); break;
}
}
else if (letter == 'M') {
switch ((int) value) {
case 2: gcodep_finish_program_and_reset(p); break;
case 17: cb->motors_enable(userdata, 1); break;
case 18: cb->motors_enable(userdata, 0); break;
case 30: gcodep_finish_program_and_reset(p); break;
case 82: p->axis_is_absolute[AXIS_E] = 1; break;
case 83: p->axis_is_absolute[AXIS_E] = 0; break;
case 84: cb->motors_enable(userdata, 0); break;
case 104: line = set_param(p, 'S', cb->set_temperature, 1.0f, line); break;
case 106: line = set_param(p, 'S', cb->set_fanspeed, 1.0f, line); break;
case 107: cb->set_fanspeed(userdata, 0); break;
case 109:
line = set_param(p, 'S', cb->set_temperature, 1.0f, line);
cb->wait_temperature(userdata);
break;
case 116: cb->wait_temperature(userdata); break;
case 220:
line = set_param(p, 'S', cb->set_speed_factor, 0.01f, line);
break;
default: line = cb->unprocessed(userdata, letter, value, line); break;
}
}
else if (letter == 'N') {
// Line number? Yeah, ignore for now :)
processed_command = 0;
}
else {
const enum GCodeParserAxis axis = gcodep_letter2axis(letter);
if (axis == GCODE_NUM_AXES) {
line = cb->unprocessed(userdata, letter, value, line);
} else {
// This line must be a continuation of a previous G0/G1 command.
// Update the axis position then handle the move.
const float unit_value = value * p->unit_to_mm_factor;
p->axes_pos[axis] = abs_axis_pos(p, axis, unit_value);
line = handle_move(p, line, 1);
// make gcode_command_done() think this was a 'G0/G1' command
letter = 'G';
value = p->modal_g0_g1;
}
}
if (processed_command) {
cb->gcode_command_done(userdata, letter, value);
}
}
p->err_msg = NULL;
}
