yair/grbl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge chamnit/v0_7 with grbl/master

Browse Source
This commit is contained in:
Sonny Jeon
2012-01-17 20:50:53 -07:00
parent 74576a8a0c
commit 9713f9067d
32 changed files with 1558 additions and 1020 deletions
Download Patch File Download Diff File Expand all files Collapse all files

200
gcode.c
View File

@@ -3,7 +3,8 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Copyright (c) 2011 Sungeun K. Jeon
Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
@@ -22,7 +23,6 @@
by Kramer, Proctor and Messina. */
#include "gcode.h"
#include <stdlib.h>
#include <string.h>
#include "nuts_bolts.h"
#include <math.h>
@@ -30,13 +30,14 @@
#include "motion_control.h"
#include "spindle_control.h"
#include "errno.h"
#include "serial_protocol.h"
#include "protocol.h"
#define MM_PER_INCH (25.4)
#define NEXT_ACTION_DEFAULT 0
#define NEXT_ACTION_DWELL 1
#define NEXT_ACTION_GO_HOME 2
#define NEXT_ACTION_SET_COORDINATE_OFFSET 3
#define MOTION_MODE_SEEK 0 // G0
#define MOTION_MODE_LINEAR 1 // G1
@@ -63,7 +64,7 @@ typedef struct {
uint8_t inches_mode; /* 0 = millimeter mode, 1 = inches mode {G20, G21} */
uint8_t absolute_mode; /* 0 = relative motion, 1 = absolute motion {G90, G91} */
uint8_t program_flow;
int spindle_direction;
int8_t spindle_direction;
double feed_rate, seek_rate; /* Millimeters/second */
double position[3]; /* Where the interpreter considers the tool to be at this point in the code */
uint8_t tool;
@@ -76,14 +77,9 @@ static parser_state_t gc;
#define FAIL(status) gc.status_code = status;
int read_double(char *line, // <- string: line of RS274/NGC code being processed
int *char_counter, // <- pointer to a counter for position on the line
double *double_ptr); // <- pointer to double to be read
static int next_statement(char *letter, double *double_ptr, char *line, uint8_t *char_counter);
int next_statement(char *letter, double *double_ptr, char *line, int *char_counter);
void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
static void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
{
gc.plane_axis_0 = axis_0;
gc.plane_axis_1 = axis_1;
@@ -92,75 +88,37 @@ void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2)
void gc_init() {
memset(&gc, 0, sizeof(gc));
gc.feed_rate = settings.default_feed_rate/60;
gc.seek_rate = settings.default_seek_rate/60;
gc.feed_rate = settings.default_feed_rate;
gc.seek_rate = settings.default_seek_rate;
select_plane(X_AXIS, Y_AXIS, Z_AXIS);
gc.absolute_mode = TRUE;
gc.absolute_mode = true;
}
inline float to_millimeters(double value) {
static float to_millimeters(double value) {
return(gc.inches_mode ? (value * MM_PER_INCH) : value);
}
// Find the angle in radians of deviance from the positive y axis. negative angles to the left of y-axis,
// positive to the right.
double theta(double x, double y)
{
double theta = atan(x/fabs(y));
if (y>0) {
return(theta);
} else {
if (theta>0)
{
return(M_PI-theta);
} else {
return(-M_PI-theta);
}
}
}
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace).
// characters and signed floating point values (no whitespace). Comments and block delete
// characters have been removed.
uint8_t gc_execute_line(char *line) {
int char_counter = 0;
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 radius_mode = false;
uint8_t absolute_override = FALSE; /* 1 = absolute motion for this block only {G53} */
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 = GCSTATUS_OK;
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
// If the line starts with an '$' it is a configuration-command
if (line[0] == '$') {
// Parameter lines are on the form '$4=374.3' or '$' to dump current settings
char_counter = 1;
if(line[char_counter] == 0) { settings_dump(); return(GCSTATUS_OK); }
read_double(line, &char_counter, &p);
if(line[char_counter++] != '=') { return(GCSTATUS_UNSUPPORTED_STATEMENT); }
read_double(line, &char_counter, &value);
if(line[char_counter] != 0) { return(GCSTATUS_UNSUPPORTED_STATEMENT); }
settings_store_setting(p, value);
return(gc.status_code);
}
/* We'll handle this as g-code. First: parse all statements */
// Pass 1: Commands
while(next_statement(&letter, &value, line, &char_counter)) {
int_value = trunc(value);
@@ -177,16 +135,17 @@ uint8_t gc_execute_line(char *line) {
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 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 93: gc.inverse_feed_rate_mode = TRUE; break;
case 94: gc.inverse_feed_rate_mode = FALSE; break;
default: FAIL(GCSTATUS_UNSUPPORTED_STATEMENT);
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;
@@ -197,7 +156,7 @@ uint8_t gc_execute_line(char *line) {
case 3: gc.spindle_direction = 1; break;
case 4: gc.spindle_direction = -1; break;
case 5: gc.spindle_direction = 0; break;
default: FAIL(GCSTATUS_UNSUPPORTED_STATEMENT);
default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
}
break;
case 'T': gc.tool = trunc(value); break;
@@ -209,6 +168,7 @@ uint8_t gc_execute_line(char *line) {
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
@@ -218,19 +178,20 @@ uint8_t gc_execute_line(char *line) {
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/60;
gc.seek_rate = unit_converted_value;
} else {
gc.feed_rate = unit_converted_value/60; // millimeters pr second
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 '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) {
@@ -246,21 +207,20 @@ uint8_t gc_execute_line(char *line) {
if (gc.status_code) { return(gc.status_code); }
// Update spindle state
if (gc.spindle_direction) {
spindle_run(gc.spindle_direction, gc.spindle_speed);
} else {
spindle_stop();
}
spindle_run(gc.spindle_direction, gc.spindle_speed);
// Perform any physical actions
switch (next_action) {
case NEXT_ACTION_GO_HOME: mc_go_home(); break;
case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break;
case NEXT_ACTION_GO_HOME: mc_go_home(); clear_vector(target); 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_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);
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],
@@ -327,7 +287,7 @@ uint8_t gc_execute_line(char *line) {
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(GCSTATUS_FLOATING_POINT_ERROR); return(gc.status_code); }
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; }
@@ -352,50 +312,29 @@ uint8_t gc_execute_line(char *line) {
// 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; }
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] = (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.
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));
| <- 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))
} else { // Offset mode specific computations
r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
*/
// 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];
// 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(theta_start, angular_travel, radius, depth, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
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,
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);
r, isclockwise);
break;
#endif
}
@@ -411,40 +350,24 @@ uint8_t gc_execute_line(char *line) {
// Parses the next statement and leaves the counter on the first character following
// the statement. Returns 1 if there was a statements, 0 if end of string was reached
// or there was an error (check state.status_code).
int next_statement(char *letter, double *double_ptr, char *line, int *char_counter) {
static int next_statement(char *letter, double *double_ptr, char *line, uint8_t *char_counter) {
if (line[*char_counter] == 0) {
return(0); // No more statements
}
*letter = line[*char_counter];
if((*letter < 'A') || (*letter > 'Z')) {
FAIL(GCSTATUS_EXPECTED_COMMAND_LETTER);
FAIL(STATUS_EXPECTED_COMMAND_LETTER);
return(0);
}
(*char_counter)++;
if (!read_double(line, char_counter, double_ptr)) {
FAIL(STATUS_BAD_NUMBER_FORMAT);
return(0);
};
return(1);
}
int read_double(char *line, //!< string: line of RS274/NGC code being processed
int *char_counter, //!< pointer to a counter for position on the line
double *double_ptr) //!< pointer to double to be read
{
char *start = line + *char_counter;
char *end;
*double_ptr = strtod(start, &end);
if(end == start) {
FAIL(GCSTATUS_BAD_NUMBER_FORMAT);
return(0);
};
*char_counter = end - line;
return(1);
}
/*
Intentionally not supported:
@@ -464,4 +387,3 @@ int read_double(char *line, //!< string: line of RS274/NGC code be
group 12 = {G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3} coordinate system selection
group 13 = {G61, G61.1, G64} path control mode
*/