gcode support for offset arcs

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:

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

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;

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 <http://www.gnu.org/licenses/>.
+*/
+
+#include <math.h>
+
+// 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);
+}

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 <http://www.gnu.org/licenses/>.
+*/
+
+#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

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