arc code complete with support for both R and IJK style blocks

arc_algorithm/arc.rb

@@ -244,160 +244,6 @@ class CircleTest
     return {:tx => tx, :ty => ty, :x => x, :y => y}
   end
 
-  # A DDA-direct search circle interpolator unrolled for each octant. Optimal and impure
-  def arc_unrolled(theta, angular_travel, radius) 
-    radius = radius
-    x = (sin(theta)*radius).round
-    y = (cos(theta)*radius).round
-    angular_direction = angular_travel.sign
-    tx = (sin(theta+angular_travel)*(radius-0.5)).floor
-    ty = (cos(theta+angular_travel)*(radius-0.5)).floor
-    f = (x**2 + y**2 - radius**2).round
-    x2 = 2*x
-    y2 = 2*y
-    dx = (y==0) ? -x.sign : y.sign*angular_direction
-    dy = (x==0) ? -y.sign : -x.sign*angular_direction
-    
-    max_steps = (angular_travel.abs*radius*2).floor
-    
-    # Quandrants of the circls
-    #       \ 1|2 /
-    #      8\ | / 3
-    #         \|/
-    # ---------|-----------
-    #     7   /|\  4
-    #       /  | \  
-    #     / 6 | 5 \ 
-    #            
-    #
-    #
-    
-    if angular_direction>0 # clockwise
-      if x.abs<y.abs # quad 1,2,6,5
-        if y>0 # quad 1,2
-          while x<0 # quad 1  x+,y+
-            x += 1        
-            f += 1+x2
-            x2 += 2
-            f_diagonal = f + 1 + y2
-            if  (f.abs >= f_diagonal.abs)
-              y += 1
-              y2 += 2
-              f = f_diagonal
-            end
-          end
-          while x>=0 # quad 2, x+, y-
-            x += 1        
-            f += 1+x2
-            x2 += 2
-            f_diagonal = f + 1 - y2
-            if  (f.abs >= f_diagonal.abs)
-              y -= 1
-              y2 -= 2
-              f = f_diagonal
-            end
-          end
-        end
-        if y<=0 # quad 6, 5
-          while x<0 # quad 6 x-, y+
-            x -= 1        
-            f += 1-x2
-            x2 -= 2
-            f_diagonal = f + 1 + y2
-            if  (f.abs >= f_diagonal.abs)
-              y += 1
-              y2 += 2
-              f = f_diagonal
-            end
-          end
-          while x>=0 # quad 5 x-, y-
-            x -= 1        
-            f += 1-x2
-            x2 -= 2
-            f_diagonal = f + 1 - y2
-            if  (f.abs >= f_diagonal.abs)
-              y -= 1
-              y2 -= 2
-              f = f_diagonal
-            end
-          end
-        end
-        # Quandrants of the circls
-        #       \ 1|2 /
-        #      8\ | / 3
-        #         \|/
-        # ---------|-----------
-        #     7   /|\  4
-        #       /  | \  
-        #     / 6 | 5 \ 
-      else 3 # quad 3,4,7,8
-        if x>0 # quad 3,4
-          while y>0 # quad 3  x+,y+
-            x += 1        
-            f += 1+x2
-            x2 += 2
-            f_diagonal = f + 1 + y2
-            if  (f.abs >= f_diagonal.abs)
-              y += 1
-              y2 += 2
-              f = f_diagonal
-            end
-          end
-          while x>=0 # quad 2, x+, y-
-            x += 1        
-            f += 1+x2
-            x2 += 2
-            f_diagonal = f + 1 - y2
-            if  (f.abs >= f_diagonal.abs)
-              y -= 1
-              y2 -= 2
-              f = f_diagonal
-            end
-          end
-        end
-        if y<=0 # quad 6, 5
-          while x<0 # quad 6 x-, y+
-            x -= 1        
-            f += 1-x2
-            x2 -= 2
-            f_diagonal = f + 1 + y2
-            if  (f.abs >= f_diagonal.abs)
-              y += 1
-              y2 += 2
-              f = f_diagonal
-            end
-          end
-          while x>=0 # quad 5 x-, y-
-            x -= 1        
-            f += 1-x2
-            x2 -= 2
-            f_diagonal = f + 1 - y2
-            if  (f.abs >= f_diagonal.abs)
-              y -= 1
-              y2 -= 2
-              f = f_diagonal
-            end
-          end
-      end  
-        
-      else
-        y += dy
-        f += 1+y2*dy
-        y2 += 2*dy
-        f_diagonal = f + 1 + x2*dx
-        if  (f.abs >= f_diagonal.abs)
-          x += dx
-          dy = -x.sign*angular_direction unless x == 0
-          x2 += 2*dx
-          f = f_diagonal
-        end
-        dx = y.sign*angular_direction unless y == 0
-      end
-      break if x*ty.sign*angular_direction>=tx*ty.sign*angular_direction && y*tx.sign*angular_direction<=ty*tx.sign*angular_direction
-    end
-    plot_pixel(tx+20, -ty+20, "o")
-    return {:tx => tx, :ty => ty, :x => x, :y => y}
-  end
 
   
 end
@@ -405,16 +251,16 @@ end
 test = CircleTest.new
 test.init
 
-#test.arc_clean(0, Math::PI*2, 5)
+test.arc_clean(0, Math::PI*2, 3)
-(1..10000).each do |r|
+# (1..10000).each do |r|
-  test.init
+#   test.init
-  data = test.arc_supaoptimal(2.9, Math::PI*1, r)
+#   data = test.arc_supaoptimal(2.9, Math::PI*1, r)
-  if (data[:tx]-data[:x]).abs > 1 || (data[:ty]-data[:y]).abs > 1
+#   if (data[:tx]-data[:x]).abs > 1 || (data[:ty]-data[:y]).abs > 1
-    puts "r=#{r} fails target control" 
+#     puts "r=#{r} fails target control" 
-    pp data
+#     pp data
-    puts
+#     puts
-  end
+#   end
-end
+# end
 
 # test.init
 # data = test.arc_supaoptimal(1.1, -Math::PI, 19)

config.h

@@ -68,4 +68,7 @@
 
 #define BAUD_RATE 14400
 
+// Unrolling the arc code is faster, but costs about 830 bytes of extra code space.
+// #define UNROLLED_ARC_LOOP
+
 #endif

gcode.c

@@ -241,25 +241,100 @@ uint8_t gc_execute_line(char *line) {
       break;
       case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
       if (radius_mode) {
-        // To be implemented
+        /* 
-      } else { // ijk-mode
+          We need to calculate the center of the circle that has the designated radius and passes
-        // calculate the theta (angle) of the current point
+          through both the current position and the target position. This method calculates the following
-        double theta_start = theta(-offset[state.plane_axis_0], -offset[state.plane_axis_1]);
+          set of equations where [x,y] is the vector from current to target position, d == distance of 
-        // calculate the theta (angle) of the target point
+          that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
-        double theta_end = theta(target[state.plane_axis_0] - offset[state.plane_axis_0] - state.position[state.plane_axis_0], 
+          the center of the travel vector. This is the distance from the center of the travel vector
-          target[state.plane_axis_1] - offset[state.plane_axis_1] - state.position[state.plane_axis_1]);
+          to the center of our circle. A perpendicular to the travel vector is scaled to the length of
-        // ensure that the difference is positive so that we have clockwise travel
+          h and added to the center of the travel vector to form the new point [i,j] which will be 
-        if (theta_end < theta_start) { theta_end += 2*M_PI; }
+          the center of our circle.
-        double angular_travel = fabs(theta_end-theta_start);
+          
-        // Invert angular motion if we want a counter clockwise arc
+          d^2 == x^2 + y^2
-        if (next_action == MOTION_MODE_CCW_ARC) {
+          h^2 == r^2 + (d/2)^2
-          angular_travel = angular_travel-2*M_PI;
+          i == x/2 - y/d*h
-        }
+          j == y/2 + x/d*h
-        // Find the radius
+          
-        double radius = hypot(offset[state.plane_axis_0], offset[state.plane_axis_1]);
+                                                               O <- [i,j]
-        // Prepare the arc
+                                                            -  |
-        mc_arc(theta_start, angular_travel, radius, state.plane_axis_0, state.plane_axis_1, state.feed_rate);        
+                                                  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:
+          
+          h = sqrt(4 r^2 + x^2 + y^2)/2
+          d = sqrt(x^2 + y^2)
+          i = x/2 - (h * y)/d
+          j = y/2 + (h * x)/d
+          
+          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 can be optimized 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[state.plane_axis_0]-state.position[state.plane_axis_0];
+        double y = target[state.plane_axis_1]-state.position[state.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
+        // The anti-clockwise circle lies to the right 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.
+        if (state.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
+        offset[state.plane_axis_0] = (x-(y*h_x2_div_d))/2;
+        offset[state.plane_axis_1] = (y+(x*h_x2_div_d))/2;
+      } 
+      
+      /*
+         This segment sets up an clockwise or counterclockwise arc from the current position to the target position around 
+         the center designated by the offset vector. All theta-values measured in radians of deviance from the positive 
+         y-axis. 
+
+                            | <- theta == 0
+                          * * *                
+                        *       *                                               
+                      *           *                                             
+                      *     O ----T   <- theta == theta_end (e.g. 90 degrees: theta == PI/2)                                          
+                      *   /                                                     
+                        C   <- theta == theta_start (e.g. -145 degrees: theta == -PI*(3/4))
+
+      */
+            
+      // 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 = theta_end-theta_start;
+      // Invert angular motion if the g-code wanted a counterclockwise arc
+      if (state.motion_mode == 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;      
     }    
   }

geometry.c

@@ -36,7 +36,3 @@ double theta(double x, double y)
   }
 }
 
-double hypot(double x, double y)
-{
-  sqrt(x*x + y*y);
-}

geometry.h

@@ -24,7 +24,4 @@
 // 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

motion_control.c

@@ -58,6 +58,7 @@ struct ArcMotionParameters {
   uint8_t axis_x, axis_y;                          // maps the arc axes to stepper axes
   int32_t target[3];                               // The target position in absolute steps
   int8_t plane_steppers[3];                        // A vector with the steppers of axis_x and axis_y set to 1, the remaining 0
+  int incomplete;                                  // True if the arc has not reached its target yet
 };
 
 /* The whole state of the motion-control-system in one struct. Makes the code a little bit hard to 
@@ -81,6 +82,7 @@ uint8_t direction_bits; // The direction bits to be used with any upcoming step-
 void set_stepper_directions(int8_t *direction);
 inline void step_steppers(uint8_t *enabled);
 inline void step_axis(uint8_t axis);
+void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate);
 
 void mc_init()
 {
@@ -98,11 +100,13 @@ void mc_dwell(uint32_t milliseconds)
 // unless invert_feed_rate is true. Then the feed_rate states the number of seconds for the whole movement.
 void mc_linear_motion(double x, double y, double z, float feed_rate, int invert_feed_rate)
 {
-  state.mode = MC_MODE_LINEAR;
+  prepare_linear_motion(trunc(x*X_STEPS_PER_MM), trunc(y*Y_STEPS_PER_MM), trunc(z*Z_STEPS_PER_MM), feed_rate, invert_feed_rate);
-	state.linear.target[X_AXIS] = trunc(x*X_STEPS_PER_MM);
+}
-	state.linear.target[Y_AXIS] = trunc(y*Y_STEPS_PER_MM);
-	state.linear.target[Z_AXIS] = trunc(z*Z_STEPS_PER_MM);
 
+// Same as mc_linear_motion but accepts target in absolute step coordinates
+void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate)
+{  
+  state.mode = MC_MODE_LINEAR;
   uint8_t axis; // loop variable
 
   // Determine direction and travel magnitude for each axis
@@ -202,6 +206,7 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
   // mm/second -> microseconds/step. Assumes all axes have the same steps/mm as the x axis
   state.pace = 
     ONE_MINUTE_OF_MICROSECONDS / (feed_rate * X_STEPS_PER_MM);
+  state.arc.incomplete = true;
 }
 
 #define check_arc_target \
@@ -209,7 +214,7 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
           state.arc.target_x * state.arc.target_direction_y) && \
          (state.arc.y * state.arc.target_direction_x <= \
           state.arc.target_y * state.arc.target_direction_x)) \
-  { state.mode = MC_MODE_AT_REST; }
+  { state.arc.incomplete = false; }
 
 // Internal method used by execute_arc to trace horizontally in the general direction provided by dx and dy
 void step_arc_along_x(int8_t dx, int8_t dy) 
@@ -259,6 +264,8 @@ void map_local_arc_directions_to_stepper_directions(int8_t dx, int8_t dy)
   set_stepper_directions(direction);
 }
 
+
+
 /*
  Quandrants of the arc
        \ 7|0 /
@@ -269,6 +276,7 @@ void map_local_arc_directions_to_stepper_directions(int8_t dx, int8_t dy)
         / | \  
    x-  / 4|3 \ x+           */
 
+#ifdef UNROLLED_ARC_LOOP // This function only used by the unrolled arc loop
 // Determine within which quadrant of the circle the provided coordinate falls
 int quadrant(uint32_t x,uint32_t y)
 {
@@ -289,70 +297,93 @@ int quadrant(uint32_t x,uint32_t y)
     }
   }  
 }
+#endif
 
-// Will trace the configured arc until the target is reached. Slightly unrolled for speed.
+// Will trace the configured arc until the target is reached. 
 void execute_arc()
 {
-  int q = quadrant(state.arc.x, state.arc.y);
+  uint32_t start_x = state.arc.x;
+  uint32_t start_y = state.arc.y;
+  int dx, dy; // Trace directions
+
   // state.mode is set to 0 (MC_MODE_AT_REST) when target is reached
-  while(state.mode == MC_MODE_ARC)
+  while(state.arc.incomplete)
   {
+#ifdef UNROLLED_ARC_LOOP
+    // Unrolling the arc code is fast, but costs about 830 bytes of extra code space.
+    int q = quadrant(state.arc.x, state.arc.y);
     if (state.arc.angular_direction) {
       switch (q) {
         case 0: 
         map_local_arc_directions_to_stepper_directions(1,-1);
-        while(state.mode && (state.arc.x>state.arc.y)) { step_arc_along_x(1,-1); }
+        while(state.arc.incomplete && (state.arc.x>state.arc.y)) { step_arc_along_x(1,-1); }
         case 1: 
         map_local_arc_directions_to_stepper_directions(1,-1);
-        while(state.mode && (state.arc.y>0)) { step_arc_along_y(1,-1); }
+        while(state.arc.incomplete && (state.arc.y>0)) { step_arc_along_y(1,-1); }
         case 2: 
         map_local_arc_directions_to_stepper_directions(-1,-1);
-        while(state.mode && (state.arc.y>-state.arc.x)) { step_arc_along_y(-1,-1); }
+        while(state.arc.incomplete && (state.arc.y>-state.arc.x)) { step_arc_along_y(-1,-1); }
         case 3: 
         map_local_arc_directions_to_stepper_directions(-1,-1);
-        while(state.mode && (state.arc.x>0)) { step_arc_along_x(-1,-1); }
+        while(state.arc.incomplete && (state.arc.x>0)) { step_arc_along_x(-1,-1); }
         case 4: 
         map_local_arc_directions_to_stepper_directions(-1,1);
-        while(state.mode && (state.arc.y<state.arc.x)) { step_arc_along_x(-1,1); }
+        while(state.arc.incomplete && (state.arc.y<state.arc.x)) { step_arc_along_x(-1,1); }
         case 5: 
         map_local_arc_directions_to_stepper_directions(-1,1);
-        while(state.mode && (state.arc.y<0)) { step_arc_along_y(-1,1); }
+        while(state.arc.incomplete && (state.arc.y<0)) { step_arc_along_y(-1,1); }
         case 6: 
         map_local_arc_directions_to_stepper_directions(1,-1);
-        while(state.mode && (state.arc.y<-state.arc.x)) { step_arc_along_y(1,1); }
+        while(state.arc.incomplete && (state.arc.y<-state.arc.x)) { step_arc_along_y(1,1); }
         case 7: 
         map_local_arc_directions_to_stepper_directions(1,1);
-        while(state.mode && (state.arc.x<0)) { step_arc_along_x(1,1); }
+        while(state.arc.incomplete && (state.arc.x<0)) { step_arc_along_x(1,1); }
       }
     } else {
       switch (q) {
         case 7:
         map_local_arc_directions_to_stepper_directions(-1,-1);
-        while(state.mode && (state.arc.y>-state.arc.x)) { step_arc_along_x(-1,-1); }
+        while(state.arc.incomplete && (state.arc.y>-state.arc.x)) { step_arc_along_x(-1,-1); }
         case 6:
         map_local_arc_directions_to_stepper_directions(-1,-1);
-        while(state.mode && (state.arc.y>0))  { step_arc_along_y(-1,-1); }
+        while(state.arc.incomplete && (state.arc.y>0))  { step_arc_along_y(-1,-1); }
         case 5:
         map_local_arc_directions_to_stepper_directions(1,-1);
-        while(state.mode && (state.arc.y>state.arc.x))  { step_arc_along_y(1,-1); }
+        while(state.arc.incomplete && (state.arc.y>state.arc.x))  { step_arc_along_y(1,-1); }
         case 4:
         map_local_arc_directions_to_stepper_directions(1,-1);
-        while(state.mode && (state.arc.x<0))  { step_arc_along_x(1,-1); }
+        while(state.arc.incomplete && (state.arc.x<0))  { step_arc_along_x(1,-1); }
         case 3:
         map_local_arc_directions_to_stepper_directions(1,1);
-        while(state.mode && (state.arc.y<-state.arc.x)) { step_arc_along_x(1,1); }      
+        while(state.arc.incomplete && (state.arc.y<-state.arc.x)) { step_arc_along_x(1,1); }      
         case 2:
         map_local_arc_directions_to_stepper_directions(1,1);
-        while(state.mode && (state.arc.y<0))  { step_arc_along_y(1,1); }      
+        while(state.arc.incomplete && (state.arc.y<0))  { step_arc_along_y(1,1); }      
         case 1:
         map_local_arc_directions_to_stepper_directions(-1,1);
-        while(state.mode && (state.arc.y<state.arc.x))  { step_arc_along_y(-1,1); }
+        while(state.arc.incomplete && (state.arc.y<state.arc.x))  { step_arc_along_y(-1,1); }
         case 0:
         map_local_arc_directions_to_stepper_directions(-1,1);
-        while(state.mode && (state.arc.x>0))  { step_arc_along_x(-1,1); }
+        while(state.arc.incomplete && (state.arc.x>0))  { step_arc_along_x(-1,1); }
       }    
     }
+#else 
+    dx = (state.arc.y!=0) ?  sign(state.arc.y) * state.arc.angular_direction : -sign(state.arc.x);
+    dy = (state.arc.x!=0) ? -sign(state.arc.x) * state.arc.angular_direction : -sign(state.arc.y);
+    if (fabs(state.arc.x)<fabs(state.arc.y)) {
+      step_arc_along_x(dx,dy);    
+    } else {
+      step_arc_along_y(dx,dy);    
+    }
+#endif    
   }
+  // Update the tool position to the new actual position
+  state.position[state.arc.axis_x] += state.arc.x-start_x;
+  state.position[state.arc.axis_y] += state.arc.y-start_y;
+  // Because of rounding errors we might be off by a step or two. Adjust for this
+    // To be implemented
+  //void prepare_linear_motion(uint32_t x, uint32_t y, uint32_t z, float feed_rate, int invert_feed_rate)
+    
 }
 
 void mc_go_home()