Fixed some arc bugs

Marlin/Marlin.pde

@@ -1139,8 +1139,8 @@ inline void get_coordinates()
 inline void get_arc_coordinates()
 {
    get_coordinates();
-   if(code_seen("I")) offset[0] = code_value();
+   if(code_seen('I')) offset[0] = code_value();
-   if(code_seen("J")) offset[1] = code_value();
+   if(code_seen('J')) offset[1] = code_value();
 }
 
 void prepare_move()
@@ -1152,119 +1152,16 @@ 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++) {
+  for(int i=0; i < NUM_AXIS; i++) {
-    current_position[ii] = destination[ii];
+    current_position[i] = destination[i];
   }
 }
 

Marlin/motion_control.cpp

@@ -19,12 +19,8 @@
   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"
 
@@ -35,10 +31,10 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8
 {      
   //   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_ECHOLN("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 extruder_travel = target[E_AXIS] - position[E_AXIS];
   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;
@@ -60,6 +56,7 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8
   */
   float theta_per_segment = angular_travel/segments;
   float linear_per_segment = linear_travel/segments;
+  float extruder_per_segment = extruder_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.
@@ -90,7 +87,7 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8
   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 arc_target[4];
   float sin_Ti;
   float cos_Ti;
   float r_axisi;
@@ -100,6 +97,9 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8
   // Initialize the linear axis
   arc_target[axis_linear] = position[axis_linear];
   
+  // Initialize the extruder axis
+  arc_target[E_AXIS] = position[E_AXIS];
+
   for (i = 1; i<segments; i++) { // Increment (segments-1)
     
     if (count < N_ARC_CORRECTION) {
@@ -122,6 +122,7 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8
     arc_target[axis_0] = center_axis0 + r_axis0;
     arc_target[axis_1] = center_axis1 + r_axis1;
     arc_target[axis_linear] += linear_per_segment;
+    arc_target[E_AXIS] += extruder_per_segment;
     plan_buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], target[E_AXIS], feed_rate);
     
   }