Refactored line buffering to eliminate state from motion control and centralize tracking of position. UNTESTED: NEEDS TESTING

main.c

@@ -37,7 +37,6 @@ int main(void)
   settings_init();
   plan_init(); // initialize the stepper plan subsystem
   st_init(); // initialize the stepper subsystem
-  mc_init(); // initialize motion control subsystem
   spindle_init(); // initialize spindle controller
   gc_init(); // initialize gcode-parser
   

motion_control.c

@@ -29,13 +29,6 @@
 #include "stepper_plan.h"
 #include "wiring_serial.h"
 
-// The current position of the tool in absolute steps
-int32_t position[3];   
-
-void mc_init()
-{
-  clear_vector(position);
-}
 
 void mc_dwell(uint32_t milliseconds) 
 {
@@ -48,31 +41,7 @@ void mc_dwell(uint32_t milliseconds)
 // 1/feed_rate minutes.
 void mc_line(double x, double y, double z, double feed_rate, int invert_feed_rate)
 {
-  uint8_t axis; // loop variable
+  st_buffer_line(x, y, z, feed_rate, invert_feed_rate);
-  int32_t target[3]; // The target position in absolute steps
-  int32_t steps[3]; // The target line in relative steps
-  
-  target[X_AXIS] = lround(x*settings.steps_per_mm[0]);
-  target[Y_AXIS] = lround(y*settings.steps_per_mm[1]);
-  target[Z_AXIS] = lround(z*settings.steps_per_mm[2]); 
-
-  for(axis = X_AXIS; axis <= Z_AXIS; axis++) {
-    steps[axis] = target[axis]-position[axis];
-  }
-  
-	// Ask old Phytagoras to estimate how many mm our next move is going to take us
-	double millimeters_of_travel = sqrt(
-	  square(steps[X_AXIS]/settings.steps_per_mm[0]) + 
-	  square(steps[Y_AXIS]/settings.steps_per_mm[1]) + 
-	  square(steps[Z_AXIS]/settings.steps_per_mm[2]));
-	if (invert_feed_rate) {
-    st_buffer_line(steps[X_AXIS], steps[Y_AXIS], steps[Z_AXIS], lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate), 
-      millimeters_of_travel);
-	} else {
-    st_buffer_line(steps[X_AXIS], steps[Y_AXIS], steps[Z_AXIS],
-      lround((millimeters_of_travel/feed_rate)*1000000), millimeters_of_travel);
-	}
-	memcpy(position, target, sizeof(target)); // position[] = target[] 
 }
 
 // Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc,
@@ -85,6 +54,8 @@ void mc_line(double x, double y, double z, double feed_rate, int invert_feed_rat
 void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, 
   int axis_linear, double feed_rate, int invert_feed_rate)
 {      
+  int32_t position[3];
+  st_get_position_steps(&position);
   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
   double millimeters_of_travel = hypot(angular_travel*radius, labs(linear_travel));
@@ -119,5 +90,4 @@ void mc_arc(double theta, double angular_travel, double radius, double linear_tr
 void mc_go_home()
 {
   st_go_home();
-  clear_vector(position); // By definition this is location [0, 0, 0]
 }

motion_control.h

@@ -23,10 +23,6 @@
 
 #include <avr/io.h>
 
-
-// Initializes the motion_control subsystem resources
-void mc_init();
-
 // Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
 // unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
 // (1 minute)/feed_rate time.

stepper.c

@@ -110,12 +110,16 @@ inline void trapezoid_generator_tick() {
 // Add a new linear movement to the buffer. steps_x, _y and _z is the signed, relative motion in 
 // steps. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 // calculation the caller must also provide the physical length of the line in millimeters.
-void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters) {
+void st_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate) {
-  plan_buffer_line(steps_x, steps_y, steps_z, microseconds, millimeters);
+  plan_buffer_line(x, y, z, feed_rate, invert_feed_rate);
   // Ensure that block processing is running by enabling The Stepper Driver Interrupt
   ENABLE_STEPPER_DRIVER_INTERRUPT();
 }
 
+void st_get_position_steps(int32_t (*vector)[3]) {
+  memcpy(vector, position, sizeof(position)); // vector[] = position[]
+}
+
 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is  executed at the rate set with
 // config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. 
 // It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse.

stepper.h

@@ -29,7 +29,10 @@ void st_init();
 
 // Add a new linear movement to the buffer. steps_x, _y and _z is the signed, relative motion in 
 // steps. Microseconds specify how many microseconds the move should take to perform.
-void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t rate, double millimeters);
+void st_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate);
+
+// Copy the current absolute position in steps into the provided vector
+void st_get_position_steps(int32_t (*vector)[3]);
 
 // Block until all buffered steps are executed
 void st_synchronize();

stepper_plan.c

@@ -65,6 +65,9 @@ block_t block_buffer[BLOCK_BUFFER_SIZE];  // A ring buffer for motion instructio
 volatile int block_buffer_head;           // Index of the next block to be pushed
 volatile int block_buffer_tail;           // Index of the block to process now
 
+// The current position of the tool in absolute steps
+int32_t position[3];   
+
 static uint8_t acceleration_manager_enabled;   // Acceleration management active?
 
 // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the 
@@ -334,6 +337,7 @@ void plan_init() {
   block_buffer_head = 0;
   block_buffer_tail = 0;
   plan_set_acceleration_manager_enabled(TRUE);
+  clear_vector(position);
 }
 
 void plan_set_acceleration_manager_enabled(int enabled) {
@@ -347,10 +351,18 @@ int plan_is_acceleration_manager_enabled() {
   return(acceleration_manager_enabled);
 }
 
-// Add a new linear movement to the buffer. steps_x, _y and _z is the signed, relative motion in 
+// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in 
-// steps. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
+// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 // calculation the caller must also provide the physical length of the line in millimeters.
-void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters) {
+void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate) {
+  // The target position of the tool in absolute steps
+  
+  // Calculate target position in absolute steps
+  int32_t target[3];
+  target[0] = lround(x*settings.steps_per_mm[0]);
+  target[1] = lround(y*settings.steps_per_mm[1]);
+  target[2] = lround(y*settings.steps_per_mm[2]);     
+  
   // Calculate the buffer head after we push this byte
 	int next_buffer_head = (block_buffer_head + 1) % BLOCK_BUFFER_SIZE;	
 	// If the buffer is full: good! That means we are well ahead of the robot. 
@@ -359,25 +371,37 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_
   // Prepare to set up new block
   block_t *block = &block_buffer[block_buffer_head];
   // Number of steps for each axis
-  block->steps_x = labs(steps_x);
+  block->steps_x = labs(position[0]-target[0]);
-  block->steps_y = labs(steps_y);
+  block->steps_y = labs(position[1]-target[1]);
-  block->steps_z = labs(steps_z);   
+  block->steps_z = labs(position[2]-target[2]);
+  block->millimeters = sqrt(
+    square(block->steps_x/settings.steps_per_mm[0])+
+    square(block->steps_y/settings.steps_per_mm[1])+
+    square(block->steps_z/settings.steps_per_mm[2]));
+	
   block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z));
   // Bail if this is a zero-length block
   if (block->step_event_count == 0) { return; };
+  
+  uint32_t microseconds;
+  if (!invert_feed_rate) {
+    microseconds = lround((block->millimeters/feed_rate)*1000000);
+  } else {
+    microseconds = lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate);
+  }
+  
   // Calculate speed in mm/minute for each axis
   double multiplier = 60.0*1000000.0/microseconds;
   // printInteger(multiplier*1000); printString("<-multi\n\r");
-  block->speed_x = steps_x*multiplier/settings.steps_per_mm[0];
+  block->speed_x = x*multiplier;
-  block->speed_y = steps_y*multiplier/settings.steps_per_mm[1];
+  block->speed_y = y*multiplier;
-  block->speed_z = steps_z*multiplier/settings.steps_per_mm[2];
+  block->speed_z = z*multiplier;
-  block->nominal_speed = millimeters*multiplier;
+  block->nominal_speed = block->millimeters*multiplier;
   // printInteger(millimeters*1000); printString("<-mm\n\r");
   // printInteger(block->nominal_speed*1000); printString("<-ns\n\r");
   block->nominal_rate = ceil(block->step_event_count*multiplier);  
   // printInteger(block->nominal_rate*1000); printString("<-nr\n\r");   
   // printInteger((uint16_t)block); printString("<-addr\n\r");
-  block->millimeters = millimeters;
   block->entry_factor = 0.0;
   
   // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
@@ -386,13 +410,14 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_
   // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
   // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed 
   // specifically for each line to compensate for this phenomenon:
-  double travel_per_step = millimeters/block->step_event_count;
+  double travel_per_step = block->millimeters/block->step_event_count;
   block->rate_delta = ceil(
     ((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick
     travel_per_step);                                               // convert to: acceleration steps/min/acceleration_tick    
   if (acceleration_manager_enabled) {
+    // compute a preliminary conservative acceleration trapezoid
     double safe_speed_factor = factor_for_safe_speed(block);
-    calculate_trapezoid_for_block(block, safe_speed_factor, safe_speed_factor);                       // compute a conservative acceleration trapezoid for now
+    calculate_trapezoid_for_block(block, safe_speed_factor, safe_speed_factor); 
   } else {
     block->initial_rate = block->nominal_rate;
     block->accelerate_until = 0;
@@ -402,16 +427,15 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_
   
   // Compute direction bits for this block
   block->direction_bits = 0;
-  if (steps_x < 0) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
+  if (target[0] < position[0]) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
-  if (steps_y < 0) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
+  if (target[1] < position[1]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
-  if (steps_z < 0) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
+  if (target[2] < position[2]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
-  // Move buffer head
-  block_buffer_head = next_buffer_head;
   
-  if (acceleration_manager_enabled) {
+  // Move buffer head
-    planner_recalculate();  
+  block_buffer_head = next_buffer_head;     
-  } else {    
+  // Update position 
-    calculate_trapezoid_for_block(block, 1.0, 1.0);
+  memcpy(position, target, sizeof(target)); // position[] = target[]
-  }  
+  
+  if (acceleration_manager_enabled) { planner_recalculate(); }
 }
 

stepper_plan.h

@@ -18,6 +18,9 @@
   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */
 
+// This module is to be considered a sub-module of stepper.c. Please don't include 
+// this file from any other module.
+
 #ifndef stepper_plan_h
 #define stepper_plan_h
                  
@@ -56,6 +59,7 @@ typedef struct {
 extern block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
 extern volatile int block_buffer_head;           // Index of the next block to be pushed
 extern volatile int block_buffer_tail;           // Index of the block to process now
+extern int32_t position[3];
 
 // Initialize the motion plan subsystem      
 void plan_init();
@@ -65,7 +69,7 @@ void plan_init();
 // Add a new linear movement to the buffer. steps_x, _y and _z is the signed, relative motion in 
 // steps. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 // calculation the caller must also provide the physical length of the line in millimeters.
-void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters);
+void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate);
 
 // Enables acceleration-management for upcoming blocks
 void plan_set_acceleration_manager_enabled(int enabled);