diff --git a/Makefile b/Makefile
index 3dd6068..ccab947 100644
--- a/Makefile
+++ b/Makefile
@@ -30,7 +30,7 @@
DEVICE = atmega168
CLOCK = 20000000
PROGRAMMER = -c avrisp2 -P usb
-OBJECTS = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o stepper.o
+OBJECTS = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o stepper.o geometry.o
FUSES = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m
# Tune the lines below only if you know what you are doing:
diff --git a/arc_algorithm/theta.rb b/arc_algorithm/theta.rb
new file mode 100644
index 0000000..9365ffb
--- /dev/null
+++ b/arc_algorithm/theta.rb
@@ -0,0 +1,16 @@
+require 'pp'
+include Math
+
+def calc_theta(x,y)
+ theta = atan(1.0*x/y.abs)
+ return(theta) if(y>0)
+ if (theta>0)
+ return(PI-theta)
+ else
+ return(-PI-theta)
+ end
+end
+
+(-180..180).each do |deg|
+ pp [deg, calc_theta(sin(1.0*deg/180*PI), cos(1.0*deg/180*PI))/PI*180]
+end
\ No newline at end of file
diff --git a/gcode.c b/gcode.c
index 89cd4f4..48466c6 100644
--- a/gcode.c
+++ b/gcode.c
@@ -50,6 +50,7 @@
#include "config.h"
#include "motion_control.h"
#include "spindle_control.h"
+#include "geometry.h"
#define NEXT_ACTION_DEFAULT 0
#define NEXT_ACTION_DWELL 1
@@ -61,10 +62,6 @@
#define MOTION_MODE_CCW_ARC 3 // G3
#define MOTION_MODE_CANCEL 4 // G80
-#define PLANE_XY 0; // G17
-#define PLANE_XZ 1; // G18
-#define PLANE_YZ 2; // G19
-
#define PATH_CONTROL_MODE_EXACT_PATH 0
#define PATH_CONTROL_MODE_EXACT_STOP 1
#define PATH_CONTROL_MODE_CONTINOUS 2
@@ -84,21 +81,22 @@ struct ParserState {
uint8_t motion_mode:3; /* {G0, G1, G2, G3, G38.2, G80, G81, G82, G83, G84, G85, G86, G87, G88, G89} */
uint8_t inverse_feed_rate_mode:1; /* G93, G94 */
- uint8_t plane:2; /* {G17, G18, G19} */
uint8_t inches_mode:1; /* 0 = millimeter mode, 1 = inches mode {G20, G21} */
uint8_t program_flow:2;
int spindle_direction:2;
double feed_rate; /* Millimeters/second */
- double logical_position[3]; /* Where the interpreter considers the tool to be at this point in the code */
+ double position[3]; /* Where the interpreter considers the tool to be at this point in the code */
uint8_t tool;
int16_t spindle_speed; /* RPM/100 */
+ uint8_t plane_axis_0, plane_axis_1; // The axes of the selected plane
+
};
struct ParserState state;
#define FAIL(status) state.status_code = status;
int read_double(char *line, //!< string: line of RS274/NGC code being processed
- int *counter, //!< pointer to a counter for logical_position on the line
+ int *counter, //!< pointer to a counter for position on the line
double *double_ptr); //!< pointer to double to be read
int next_statement(char *letter, double *double_ptr, char *line, int *counter);
@@ -113,6 +111,12 @@ inline float to_millimeters(double value) {
return(state.inches_mode ? value * INCHES_PER_MM : value);
}
+void select_plane(uint8_t axis_0, uint8_t axis_1)
+{
+ state.plane_axis_0 = axis_0;
+ state.plane_axis_1 = axis_1;
+}
+
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floats (no whitespace).
uint8_t gc_execute_line(char *line) {
@@ -121,6 +125,7 @@ uint8_t gc_execute_line(char *line) {
double value;
double unit_converted_value;
double inverse_feed_rate;
+ int radius_mode;
uint8_t absolute_mode; /* 0 = relative motion, 1 = absolute motion {G90, G91} */
uint8_t next_action = NEXT_ACTION_DEFAULT; /* One of the NEXT_ACTION_-constants */
@@ -149,9 +154,9 @@ uint8_t gc_execute_line(char *line) {
case 2: state.motion_mode = MOTION_MODE_CW_ARC; break;
case 3: state.motion_mode = MOTION_MODE_CCW_ARC; break;
case 4: next_action = NEXT_ACTION_DWELL; break;
- case 17: state.plane = PLANE_XY; break;
- case 18: state.plane = PLANE_XZ; break;
- case 19: state.plane = PLANE_YZ; break;
+ case 17: select_plane(X_AXIS, Y_AXIS); break;
+ case 18: select_plane(X_AXIS, Z_AXIS); break;
+ case 19: select_plane(Y_AXIS, Z_AXIS); break;
case 20: state.inches_mode = true; break;
case 21: state.inches_mode = false; break;
case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break;
@@ -197,14 +202,14 @@ uint8_t gc_execute_line(char *line) {
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; break;
+ case 'R': r = unit_converted_value; radius_mode = true; break;
case 'S': state.spindle_speed = value; break;
case 'X': case 'Y': case 'Z':
axis = letter - 'X';
if (absolute_mode) {
target[axis] = unit_converted_value;
} else {
- target[axis] = state.logical_position[axis]+unit_converted_value;
+ target[axis] = state.position[axis]+unit_converted_value;
};
break;
}
@@ -235,17 +240,36 @@ uint8_t gc_execute_line(char *line) {
}
break;
case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
- // to be implemented
+ if (radius_mode) {
+ // To be implemented
+ } else { // ijk-mode
+ // calculate the theta (angle) of the current point
+ double theta_start = theta(-offset[state.plane_axis_0], -offset[state.plane_axis_1]);
+ // calculate the theta (angle) of the target point
+ double theta_end = theta(target[state.plane_axis_0] - offset[state.plane_axis_0] - state.position[state.plane_axis_0],
+ target[state.plane_axis_1] - offset[state.plane_axis_1] - state.position[state.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 = fabs(theta_end-theta_start);
+ // Invert angular motion if we want a counter clockwise arc
+ if (next_action == MOTION_MODE_CCW_ARC) {
+ angular_travel = angular_travel-2*M_PI;
+ }
+ // Find the radius
+ double radius = hypot(offset[state.plane_axis_0], offset[state.plane_axis_1]);
+ // Prepare the arc
+ mc_arc(theta_start, angular_travel, radius, state.plane_axis_0, state.plane_axis_1, state.feed_rate);
+ }
break;
}
}
mc_execute();
- // As far as the parser is concerned, the logical_position is now == target. In reality the
+ // 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(state.logical_position, target, sizeof(state.logical_position));
+ memcpy(state.position, target, sizeof(state.position));
return(state.status_code);
}
@@ -255,10 +279,10 @@ void gc_get_status(double *position, uint8_t *status_code, int *inches_mode, uin
int axis;
if (state.inches_mode) {
for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
- position[axis] = state.logical_position[axis]*INCHES_PER_MM;
+ position[axis] = state.position[axis]*INCHES_PER_MM;
}
} else {
- memcpy(position, state.logical_position, sizeof(position));
+ memcpy(position, state.position, sizeof(position));
}
*status_code = state.status_code;
*inches_mode = state.inches_mode;
diff --git a/geometry.c b/geometry.c
new file mode 100644
index 0000000..3b9c27e
--- /dev/null
+++ b/geometry.c
@@ -0,0 +1,42 @@
+/*
+ geometry.h - a place for geometry helpers
+ Part of Grbl
+
+ Copyright (c) 2009 Simen Svale Skogsrud
+
+ 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
+ (at your option) any later version.
+
+ Grbl is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Grbl. If not, see .
+*/
+
+#include
+
+// Find the angle from the positive y axis to the given point with respect to origo.
+double theta(double x, double y)
+{
+ double theta = atan(x/fabs(y));
+ if (y>0) {
+ return(theta);
+ } else {
+ if (theta>0)
+ {
+ return(theta-M_PI);
+ } else {
+ return(-M_PI-theta);
+ }
+ }
+}
+
+double hypot(double x, double y)
+{
+ sqrt(x*x + y*y);
+}
diff --git a/geometry.h b/geometry.h
new file mode 100644
index 0000000..460d966
--- /dev/null
+++ b/geometry.h
@@ -0,0 +1,30 @@
+/*
+ geometry.h - a place for geometry helpers
+ Part of Grbl
+
+ Copyright (c) 2009 Simen Svale Skogsrud
+
+ 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
+ (at your option) any later version.
+
+ Grbl is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Grbl. If not, see .
+*/
+
+#ifndef geometry_h
+#define geometry_h
+
+// Find the angle from the positive y axis to the given point with respect to origo.
+double theta(double x, double y);
+
+// Find the distance from origo to point [x,y]
+double hypot(double x, double y);
+
+#endif
\ No newline at end of file
diff --git a/todo.txt b/todo.txt
index a687095..6de8cc2 100644
--- a/todo.txt
+++ b/todo.txt
@@ -1,3 +1,4 @@
+* Generalize feed rate code and support inverse feed rate for arcs
* Implement homing cycle in stepper.c
* Implement limit switch support in stepper.c (use port-triggered interrupts?)
* How to implement st_set_pace? Consider synchronizing when pace is changed