added code to estimate steps in arc in order to support helical motion

geometry.c

@@ -18,7 +18,10 @@
   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */
 
+#include "geometry.h"
+#include <avr/io.h>
 #include <math.h>
+#include <stdlib.h>
 
 // Find the angle in radians of deviance from the positive y axis. negative angles to the left of y-axis, 
 // positive to the right.
@@ -36,3 +39,59 @@ double theta(double x, double y)
     }
   }
 }
+
+/* 
+  Quadrants of the circle
+    
+  +---- 0 ----+    0 - y is always positive and |x| < |y|
+  |           |    1 - x is always positive and |x| > |y|
+  |           |    2 - y is always negative and |x| < |y|
+  3     +     1    3 - x is always negative and |x| > |y|
+  |           |
+  |           |
+  +---- 2 ----+                    
+*/
+
+// Find the quadrant of the coordinate 
+int quadrant_of_the_circle(int32_t x, int32_t y) {
+  if (abs(x)<abs(y)){
+    if (y>0) {
+      return(0);
+    } else {
+      return(2);
+    }      
+  } else {
+    if (x>0) {
+      return(1);
+    } else {
+      return(3);
+    }      
+  }
+}
+
+// Very specialized helper to calculate the amount of steps to travel in the given quadrant of a circle provided the
+// axial direction of the quadrant, the angular_direction of travel (-1 or +1) and amount of steps in one half quadrant 
+// of the circle. 
+uint32_t steps_in_partial_quadrant(int32_t x, int32_t y, int quadrant, int angular_direction, 
+  int32_t steps_in_half_quadrant) {
+  if (quadrant_horizontal(quadrant))  { // A horizontal quadrant
+    if ((angular_direction == 1) ^ (quadrant == 2)) {
+      return(steps_in_half_quadrant-x);
+    } else {
+      return(x+steps_in_half_quadrant);
+    }
+  } else { // A vertical quadrant
+    if ((angular_direction == 1) ^ (quadrant == 3)) {
+      return(steps_in_half_quadrant-y);
+    } else {
+      return(y+steps_in_half_quadrant);
+    }
+  }
+}
+
+// Counts the amount of full quadrants between quadrant_start and quadrant_target along the angular_direction
+int full_quadrants_between(int quadrant_start, int quadrant_target, int angular_direction) {
+  int diff = angular_direction*(quadrant_target-quadrant_start);
+  if (diff <= 0) { diff += 4; }
+  return (diff-1);
+}

geometry.h

@@ -20,8 +20,36 @@
 
 #ifndef geometry_h
 #define geometry_h 
+#include <avr/io.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 quadrant of the coordinate
+int quadrant_of_the_circle(int32_t x, int32_t y);
+
+/* 
+  Quadrants of the circle
+    
+  +---- 0 ----+    0 - y is always positive and |x| < |y|
+  |           |    1 - x is always positive and |x| > |y|
+  |           |    2 - y is always negative and |x| < |y|
+  3     +     1    3 - x is always negative and |x| > |y|
+  |           |
+  |           |
+  +---- 2 ----+                    
+*/
+
+// A macro to decide if a quadrant-number represent a horizontal quadrant
+#define quadrant_horizontal(q) ((q % 2) == 0)
+
+// Very specialized helper to calculate the amount of steps to travel in the given quadrant of a circle provided the
+// axial direction of the quadrant, the angular_direction of travel (-1 or +1) and amount of steps in one half quadrant 
+// of the circle. 
+uint32_t steps_in_partial_quadrant(int32_t x, int32_t y, int horizontal_quadrant, int angular_direction, 
+  int32_t steps_in_half_quadrant);
+  
+// Counts the amount of full quadrants between quadrant_start and quadrant_target along the angular_direction
+int full_quadrants_between(int quadrant_start, int quadrant_target, int angular_direction);
+
 #endif

motion_control.c

@@ -34,6 +34,9 @@
 #include <stdlib.h>
 #include "nuts_bolts.h"
 #include "stepper.h"
+#include "geometry.h"
+
+#include "wiring_serial.h"
 
 #define ONE_MINUTE_OF_MICROSECONDS 60000000.0
 
@@ -74,7 +77,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
     maximum_steps;   // The larges absolute step-count of any axis
   
   
-  // Setup
+  // Setup ---------------------------------------------------------------------------------------------------
 
   target[X_AXIS] = x*X_STEPS_PER_MM;
   target[Y_AXIS] = y*Y_STEPS_PER_MM;
@@ -109,7 +112,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
   	st_buffer_pace(((millimeters_to_travel * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / maximum_steps);	
   }
   
-  // Execution
+  // Execution -----------------------------------------------------------------------------------------------
 
   mode = MC_MODE_LINEAR;
   
@@ -152,8 +155,7 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
                                                    // local coordinate system of the arc-generator where [0,0] is the 
                                                    // center of the arc.
   int target_direction_x, target_direction_y;      // signof(target_x)*angular_direction precalculated for speed                                                 
-  int32_t error, x2, y2;                           // error is always == (x**2 + y**2 - radius**2), 
+  int32_t error;                                   // error is always == (x**2 + y**2 - radius**2),                                                    
-                                                   // x2 is always 2*x, y2 is always 2*y
   uint8_t axis_x, axis_y;                          // maps the arc axes to stepper axes
   int8_t diagonal_bits;                            // A bitmask with the stepper bits for both selected axes set
   int incomplete;                                  // True if the arc has not reached its target yet
@@ -164,6 +166,9 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
   
   uint32_t radius_steps = round(radius*X_STEPS_PER_MM);
 	if(radius_steps == 0) { return; }
+
+  // Setup arc interpolation --------------------------------------------------------------------------------
+
   // Determine angular direction (+1 = clockwise, -1 = counterclockwise)
   angular_direction = signof(angular_travel);
   // Calculate the initial position and target position in the local coordinate system of the arc
@@ -178,26 +183,71 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
   // <0 we are inside the arc, when it is >0 we are outside of the arc, and when it is 0 we 
   // are exactly on top of the arc.
   error = x*x + y*y - radius_steps*radius_steps;
-  // Because the error-value moves in steps of (+/-)2x+1 and (+/-)2y+1 we save a couple of multiplications
-  // by keeping track of the doubles of the arc coordinates at all times.
-  x2 = 2*x;
-  y2 = 2*y;   
   // Set up a vector with the steppers we are going to use tracing the plane of this arc
   diagonal_bits = st_bit_for_stepper(axis_1);
   diagonal_bits |= st_bit_for_stepper(axis_2);
   // And map the local coordinate system of the arc onto the tool axes of the selected plane
   axis_x = axis_1;
   axis_y = axis_2;
+  
+  // Estimate length of arc in steps -------------------------------------------------------------------------
+  
+  /*  
+    To support helical motion we need to know in advance how many steppings the arc will need. 
+    The calculations are based on the fact that we trace the circle by offsetting a square. The circle has
+    four "sides" or quadrants. For each quadrant we step mainly in one axis. The amount steps for one quarter of the 
+    circle (e.g. along the x axis with positive y) is equal to one side of a square inscribed in the circle we
+    are tracing.
+
+    Quadrants of the circle
+    
+    +---- 0 ----+    0 - y is always positive and |x| < |y|
+    |           |    1 - x is always positive and |x| > |y|
+    |           |    2 - y is always negative and |x| < |y|
+    3     +     1    3 - x is always negative and |x| > |y|
+    |           |
+    |           |    length of one side: 2*radius/sqrt(2)
+    +---- 2 ----+                    
+  */
+
+  int start_quadrant = quadrant_of_the_circle(start_x, start_y);
+  int target_quadrant = quadrant_of_the_circle(target_x, target_y);
+  uint32_t steps_to_travel=0;
+  // Is the start and target point in the same quadrant?
+  if (start_quadrant == target_quadrant && (abs(angular_travel) <= (M_PI/2))) { 
+    if(quadrant_horizontal(start_quadrant)) { // a horizontal quadrant where x will be the primary direction
+      steps_to_travel = abs(target_x-start_x);
+    } else { // a vertical quadrant where y will be the primary direction
+      steps_to_travel = abs(target_y-start_y);
+    }
+  } else { // the start and target points are in different quadrants
+    // Lets estimate the amount of steps along one full quadrant
+    uint32_t steps_in_half_quadrant = ceil(radius_steps/sqrt(2));
+    // Add the steps in the first partial quadrant
+    steps_to_travel += steps_in_partial_quadrant(start_x, start_y, 
+      start_quadrant, angular_direction, steps_in_half_quadrant);
+    // Count the number of full quadrants between the start and end quadrants
+    uint8_t full_quadrants_traveled = full_quadrants_between(start_quadrant, target_quadrant, angular_direction);
+    // Add steps for the full quadrants plus some stray steps for "corners"
+    steps_to_travel += full_quadrants_traveled*(steps_in_half_quadrant*2+1);
+    // Add the steps in the final partial quadrant. By inverting the angular direction we get the correct number for
+    // the target quadrant which steps through the opposite part of the quadrant with respect to the start quadrant.
+    steps_to_travel += steps_in_partial_quadrant(target_x, target_y, 
+      target_quadrant, -angular_direction, steps_in_half_quadrant);
+  }
+  
+  // Calculate feed rate -------------------------------------------------------------------------------------
+  
   // The amount of steppings performed while tracing a half circle is equal to the sum of sides in a 
   // square inscribed in the circle. We use this to estimate the amount of steps as if this arc was a half circle:
-  uint32_t steps_in_half_circle = round(radius_steps * 4 * (1/sqrt(2)));
+  uint32_t steps_in_half_circle = round((4*radius_steps)/sqrt(2)));
   // We then calculate the millimeters of travel along the circumference of that same half circle
   double millimeters_half_circumference = radius*M_PI;
   // Then we calculate the microseconds between each step as if we will trace the full circle.
   // It doesn't matter what fraction of the circle we are actually going to trace. The pace is the same.
   st_buffer_pace(((millimeters_half_circumference * ONE_MINUTE_OF_MICROSECONDS) / feed_rate) / steps_in_half_circle);    
 
-  // Execution
+  // Execution -----------------------------------------------------------------------------------------------
 
   mode = MC_MODE_ARC;
   
@@ -214,11 +264,11 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
     // Check which axis will be "major" for this stepping
     if (abs(x)<abs(y)) {
       // Step arc horizontally      
-      error += 1+x2*dx;
+      error += 1 + 2*x * dx;
-      x+=dx; x2 += 2*dx;
+      x+=dx;
-      diagonal_error = error + 1 + y2*dy;
+      diagonal_error = error + 1 + 2*y*dy;
       if(abs(error) >= abs(diagonal_error)) {
-        y += dy; y2 += 2*dy;
+        y += dy;
         error = diagonal_error;
         step_steppers(diagonal_bits); // step diagonal
       } else {
@@ -226,11 +276,11 @@ void mc_arc(double theta, double angular_travel, double radius, int axis_1, int
       }
     } else {      
       // Step arc vertically      
-      error += 1+y2*dy; 
+      error += 1 + 2*y * dy; 
-      y+=dy; y2 += 2*dy; 
+      y+=dy;
-      diagonal_error = error + 1 + x2*dx; 
+      diagonal_error = error + 1 + 2*x * dx; 
       if(abs(error) >= abs(diagonal_error)) { 
-        x += dx; x2 += 2*dx; 
+        x += dx; 
         error = diagonal_error; 
         step_steppers(diagonal_bits); // step diagonal
       } else {
@@ -267,7 +317,7 @@ int mc_status()
 
 // Set the direction bits for the stepper motors according to the provided vector. 
 // direction is an array of three 8 bit integers representing the direction of 
-// each motor. The values should be -1 (reverse), 0 or 1 (forward).
+// each motor. The values should be negative (reverse), 0 or positive (forward).
 void set_stepper_directions(int8_t *direction) 
 {
   /* Sorry about this convoluted code! It uses the fact that bit 7 of each direction

stepper.c

@@ -112,19 +112,20 @@ void st_init()
 
 void st_buffer_step(uint8_t motor_port_bits)
 {
-  if (echo_steps && !(motor_port_bits&0x80)) {
+  // Buffer nothing unless stepping subsystem is running
+  if (stepper_mode != STEPPER_MODE_RUNNING) { return; }
   // Echo steps. If bit 7 is set, the message is internal to Grbl and should not be echoed
+  if (echo_steps && !(motor_port_bits&0x80)) {    
     printByte('!'+motor_port_bits);
   }
-
+  // Calculate the buffer head after we push this byte
-	int i = (step_buffer_head + 1) % STEP_BUFFER_SIZE;
+	int next_buffer_head = (step_buffer_head + 1) % STEP_BUFFER_SIZE;	
-	
 	// If the buffer is full: good! That means we are well ahead of the robot. 
 	// Nap until there is room for more steps.
-	while(step_buffer_tail == i) { sleep_mode(); }
+	while(step_buffer_tail == next_buffer_head) { sleep_mode(); }	
-	
+	// Push byte
   step_buffer[step_buffer_head] = motor_port_bits;
-  step_buffer_head = i;
+  step_buffer_head = next_buffer_head;
 }
 
 // Block until all buffered steps are executed
@@ -163,18 +164,24 @@ inline void st_stop()
   stepper_mode = STEPPER_MODE_STOPPED;
 }
 
+// Buffer a pace change. Pace is the rate with which steps are executed. It is measured in microseconds from step to step. 
+// It is continually adjusted to achieve constant actual feed rate. Unless pace-changes was buffered along with the steps 
+// they govern they might change at slightly wrong moments in time as the pace would change while the stepper buffer was
+// still churning out the previous movement.
 void st_buffer_pace(uint32_t microseconds)
 {
-  // Do nothing if the pace in unchanged
+  // Do nothing if the pace in unchanged or the stepping subsytem is not running
-  if (current_pace == microseconds) { return; }
+  if ((current_pace == microseconds) || (stepper_mode != STEPPER_MODE_RUNNING)) { return; }
   // If the single-element pace "buffer" is full, sleep until it is popped
   while (next_pace != 0) {
     sleep_mode();
   }  
+  // Buffer the pace change
   next_pace = microseconds;
   st_buffer_step(PACE_CHANGE_MARKER); // Place a pace-change marker in the step-buffer
 }
 
+// Returns a bitmask with the stepper bit for the given axis set
 uint8_t st_bit_for_stepper(int axis) {
   switch(axis) {
     case X_AXIS: return(1<<X_STEP_BIT);
@@ -184,6 +191,7 @@ uint8_t st_bit_for_stepper(int axis) {
   return(0);
 }
 
+// Configures the prescaler and ceiling of timer 1 to produce the given pace as accurately as possible.
 void config_pace_timer(uint32_t microseconds)
 {
   uint32_t ticks = microseconds*TICKS_PER_MICROSECOND;

stepper.h

@@ -35,7 +35,10 @@ void st_init();
 // Returns a bitmask with the stepper bit for the given axis set
 uint8_t st_bit_for_stepper(int axis);
 
-// Buffer a change in the rate steps are taken from the buffer and executed
+// Buffer a pace change. Pace is the rate with which steps are executed. It is measured in microseconds from step to step. 
+// It is continually adjusted to achieve constant actual feed rate. Unless pace-changes was buffered along with the steps 
+// they govern they might change at slightly wrong moments in time as the pace would change while the stepper buffer was
+// still churning out the previous movement.
 void st_buffer_pace(uint32_t microseconds);
 
 // Buffer a new instruction for the steppers