First arcs version. (Arcs not working ok)

Marlin/Configuration.h

@@ -3,6 +3,9 @@
 
 //#define DEBUG_STEPS
 
+#define MM_PER_ARC_SEGMENT 1
+#define N_ARC_CORRECTION 25
+
 // BASIC SETTINGS: select your board type, thermistor type, axis scaling, and endstop configuration
 
 //// The following define selects which electronics board you have. Please choose the one that matches your setup

Marlin/Marlin.pde

@@ -35,6 +35,7 @@
 #include "planner.h"
 #include "stepper.h"
 #include "temperature.h"
+#include "motion_control.h"
 
 #ifdef SIMPLE_LCD
   #include "Simplelcd.h"
@@ -113,6 +114,7 @@ float destination[NUM_AXIS] = {
   0.0, 0.0, 0.0, 0.0};
 float current_position[NUM_AXIS] = {
   0.0, 0.0, 0.0, 0.0};
+float offset[3] = {0.0, 0.0, 0.0};
 bool home_all_axis = true;
 float feedrate = 1500.0, next_feedrate, saved_feedrate;
 long gcode_N, gcode_LastN;
@@ -441,6 +443,8 @@ inline void get_command()
           switch((int)((strtod(&cmdbuffer[bufindw][strchr_pointer - cmdbuffer[bufindw] + 1], NULL)))){
           case 0:
           case 1:
+          case 2:
+          case 3:
 #ifdef SDSUPPORT
             if(savetosd)
               break;
@@ -543,6 +547,16 @@ inline void process_commands()
       //ClearToSend();
       return;
       //break;
+    case 2: // G2  - CW ARC
+      get_arc_coordinates();
+      prepare_arc_move(true);
+      previous_millis_cmd = millis();
+      return;
+    case 3: // G3  - CCW ARC
+      get_arc_coordinates();
+      prepare_arc_move(false);
+      previous_millis_cmd = millis();
+      return;
     case 4: // G4 dwell
       codenum = 0;
       if(code_seen('P')) codenum = code_value(); // milliseconds to wait
@@ -1139,6 +1153,13 @@ inline void get_coordinates()
   }
 }
 
+inline void get_arc_coordinates()
+{
+   get_coordinates();
+   if(code_seen("I")) offset[0] = code_value();
+   if(code_seen("J")) offset[1] = code_value();
+}
+
 void prepare_move()
 {
   plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60.0/100.0);
@@ -1147,7 +1168,122 @@ void prepare_move()
   }
 }
 
+void prepare_arc_move(char isclockwise) {
+#if 0
+  if (radius_mode) {
+    /* 
+      We need to calculate the center of the circle that has the designated radius and passes
+      through both the current position and the target position. This method calculates the following
+      set of equations where [x,y] is the vector from current to target position, d == magnitude of 
+      that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
+      the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the 
+      length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point 
+      [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
+      
+      d^2 == x^2 + y^2
+      h^2 == r^2 - (d/2)^2
+      i == x/2 - y/d*h
+      j == y/2 + x/d*h
+      
+                                                           O <- [i,j]
+                                                        -  |
+                                              r      -     |
+                                                  -        |
+                                               -           | h
+                                            -              |
+                              [0,0] ->  C -----------------+--------------- T  <- [x,y]
+                                        | <------ d/2 ---->|
+                
+      C - Current position
+      T - Target position
+      O - center of circle that pass through both C and T
+      d - distance from C to T
+      r - designated radius
+      h - distance from center of CT to O
+      
+      Expanding the equations:
+
+      d -> sqrt(x^2 + y^2)
+      h -> sqrt(4 * r^2 - x^2 - y^2)/2
+      i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 
+      j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
+     
+      Which can be written:
+      
+      i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
+      j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
+      
+      Which we for size and speed reasons optimize to:
+
+      h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
+      i = (x - (y * h_x2_div_d))/2
+      j = (y + (x * h_x2_div_d))/2
+      
+    */
+    
+    // Calculate the change in position along each selected axis
+    double x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
+    double y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
+    
+    clear_vector(offset);
+    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(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; }
+    
+    /* The counter clockwise circle lies to the left of the target direction. When offset is positive,
+       the left hand circle will be generated - when it is negative the right hand circle is generated.
+       
+       
+                                                         T  <-- Target position
+                                                         
+                                                         ^ 
+              Clockwise circles with this center         |          Clockwise circles with this center will have
+              will have > 180 deg of angular travel      |          < 180 deg of angular travel, which is a good thing!
+                                               \         |          /   
+  center of arc when h_x2_div_d is positive ->  x <----- | -----> x <- center of arc when h_x2_div_d is negative
+                                                         |
+                                                         |
+                                                         
+                                                         C  <-- Current position                                 */
+                
+
+    // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), 
+    // 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; 
+        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] = 0.5*(x-(y*h_x2_div_d));
+    offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
 
+  } else { // Offset mode specific computations
+#endif
+    float r = hypot(offset[X_AXIS], offset[Y_AXIS]); // Compute arc radius for mc_arc
+
+//  }
+  
+  // 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(current_position, destination, offset, X_AXIS, Y_AXIS, Z_AXIS, feedrate*feedmultiply/60.0/100.0, r, isclockwise);
+    
+//  }
+  
+  // 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.
+  for(int ii=0; ii < NUM_AXIS; ii++) {
+    current_position[ii] = destination[ii];
+  }
+}
 
 #ifdef USE_WATCHDOG
 
@@ -1219,7 +1355,7 @@ inline void kill()
   disable_e();
   
   if(PS_ON_PIN > -1) pinMode(PS_ON_PIN,INPUT);
-  Serial.println("!! Printer halted. kill() called!!");
+  Serial.println("!! Printer halted. kill() called !!");
   while(1); // Wait for reset
 }
 
@@ -1233,3 +1369,4 @@ void manage_inactivity(byte debug) {
   }
   check_axes_activity();
 }
+

Marlin/motion_control.cpp

@@ -0,0 +1,133 @@
+/*
+  motion_control.c - high level interface for issuing motion commands
+  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
+  (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 "motion_control.h"
+#include "Configuration.h"
+#include "Marlin.h"
+//#include <util/delay.h>
+//#include <math.h>
+//#include <stdlib.h>
+#include "stepper.h"
+#include "planner.h"
+
+// The arc is approximated by generating a huge number of tiny, linear segments. The length of each 
+// segment is configured in settings.mm_per_arc_segment.  
+void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8_t axis_1, 
+  uint8_t axis_linear, float feed_rate, float radius, uint8_t isclockwise)
+{      
+//   int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
+//   plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc
+  Serial.println("mc_arc");
+  float center_axis0 = position[axis_0] + offset[axis_0];
+  float center_axis1 = position[axis_1] + offset[axis_1];
+  float linear_travel = target[axis_linear] - position[axis_linear];
+  float r_axis0 = -offset[axis_0];  // Radius vector from center to current location
+  float r_axis1 = -offset[axis_1];
+  float rt_axis0 = target[axis_0] - center_axis0;
+  float rt_axis1 = target[axis_1] - center_axis1;
+  
+  // CCW angle between position and target from circle center. Only one atan2() trig computation required.
+  float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
+  if (angular_travel < 0) { angular_travel += 2*M_PI; }
+  if (isclockwise) { angular_travel -= 2*M_PI; }
+  
+  float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
+  if (millimeters_of_travel == 0.0) { return; }
+  uint16_t segments = floor(millimeters_of_travel/MM_PER_ARC_SEGMENT);
+/*  
+  // Multiply inverse feed_rate to compensate for the fact that this movement is approximated
+  // by a number of discrete segments. The inverse feed_rate should be correct for the sum of 
+  // all segments.
+  if (invert_feed_rate) { feed_rate *= segments; }
+*/
+  float theta_per_segment = angular_travel/segments;
+  float linear_per_segment = linear_travel/segments;
+  
+  /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
+     and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
+         r_T = [cos(phi) -sin(phi);
+                sin(phi)  cos(phi] * r ;
+     
+     For arc generation, the center of the circle is the axis of rotation and the radius vector is 
+     defined from the circle center to the initial position. Each line segment is formed by successive
+     vector rotations. This requires only two cos() and sin() computations to form the rotation
+     matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
+     all double numbers are single precision on the Arduino. (True double precision will not have
+     round off issues for CNC applications.) Single precision error can accumulate to be greater than
+     tool precision in some cases. Therefore, arc path correction is implemented. 
+
+     Small angle approximation may be used to reduce computation overhead further. This approximation
+     holds for everything, but very small circles and large mm_per_arc_segment values. In other words,
+     theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
+     to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for 
+     numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
+     issue for CNC machines with the single precision Arduino calculations.
+     
+     This approximation also allows mc_arc to immediately insert a line segment into the planner 
+     without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
+     a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. 
+     This is important when there are successive arc motions. 
+  */
+  // Vector rotation matrix values
+  float cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
+  float sin_T = theta_per_segment;
+  
+  float arc_target[3];
+  float sin_Ti;
+  float cos_Ti;
+  float r_axisi;
+  uint16_t i;
+  int8_t count = 0;
+
+  // Initialize the linear axis
+  arc_target[axis_linear] = position[axis_linear];
+
+  for (i = 1; i<segments; i++) { // Increment (segments-1)
+    
+    if (count < N_ARC_CORRECTION) {
+      // Apply vector rotation matrix 
+      r_axisi = r_axis0*sin_T + r_axis1*cos_T;
+      r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
+      r_axis1 = r_axisi;
+      count++;
+    } else {
+      // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
+      // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
+      cos_Ti = cos(i*theta_per_segment);
+      sin_Ti = sin(i*theta_per_segment);
+      r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
+      r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
+      count = 0;
+    }
+
+    // Update arc_target location
+    arc_target[axis_0] = center_axis0 + r_axis0;
+    arc_target[axis_1] = center_axis1 + r_axis1;
+    arc_target[axis_linear] += linear_per_segment;
+    plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], target[E_AXIS], feed_rate);
+    
+  }
+  // Ensure last segment arrives at target location.
+  plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate);
+
+//   plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
+}
+

Marlin/motion_control.h

@@ -0,0 +1,32 @@
+/*
+  motion_control.h - high level interface for issuing motion commands
+  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
+  (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 motion_control_h
+#define motion_control_h
+
+// Execute an arc in offset mode format. position == current xyz, target == target xyz, 
+// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is
+// the direction of helical travel, radius == circle radius, isclockwise boolean. Used
+// for vector transformation direction.
+void mc_arc(float *position, float *target, float *offset, unsigned char axis_0, unsigned char axis_1,
+  unsigned char axis_linear, float feed_rate, float radius, unsigned char isclockwise);
+  
+#endif