From e21064bd866b90ced187fdf0441698b6a2df42e8 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 30 Jan 2009 16:14:48 +0100 Subject: [PATCH] gcode support for offset arcs --- Makefile | 2 +- arc_algorithm/theta.rb | 16 ++++++++++++ gcode.c | 58 +++++++++++++++++++++++++++++------------- geometry.c | 42 ++++++++++++++++++++++++++++++ geometry.h | 30 ++++++++++++++++++++++ todo.txt | 1 + 6 files changed, 131 insertions(+), 18 deletions(-) create mode 100644 arc_algorithm/theta.rb create mode 100644 geometry.c create mode 100644 geometry.h 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