Optimized planner re-write. Significantly faster. Full arc support enabled by rotation matrix approach.

- Significant improvements in the planner. Removed or reordered
repetitive and expensive calculations by order of importance:
recalculating unchanged blocks, trig functions [sin(), cos(), tan()],
sqrt(), divides, and multiplications. Blocks long enough for nominal
speed to be guaranteed to be reached ignored by planner. Done by
introducing two uint8_t flags per block. Reduced computational overhead
by an order of magnitude.   - Arc motion generation completely
re-written and optimized. Now runs with acceleration planner. Removed
all but one trig function (atan2) from initialization. Streamlined
computations. Segment target locations generated by vector
transformation and small angle approximation. Arc path correction
implemented for accumulated error of approximation and single precision
calculation of Arduino. Bug fix in message passing.

gcode.c

@@ -3,6 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Modifications Copyright (c) 2011 Sungeun (Sonny) 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
@@ -97,25 +98,6 @@ static float to_millimeters(double value) {
   return(gc.inches_mode ? (value * MM_PER_INCH) : value);
 }
 
-#ifdef __AVR_ATmega328P__        
-// Find the angle in radians of deviance from the positive y axis. negative angles to the left of y-axis, 
-// positive to the right.
-static double theta(double x, double y)
-{
-  double theta = atan(x/fabs(y));
-  if (y>0) {
-    return(theta);
-  } else {
-    if (theta>0) 
-    {
-      return(M_PI-theta);
-    } else {
-      return(-M_PI-theta);
-    }
-  }
-}
-#endif
-
 // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
 // characters and signed floating point values (no whitespace). Comments and block delete
 // characters have been removed.
@@ -125,7 +107,7 @@ uint8_t gc_execute_line(char *line) {
   double value;
   double unit_converted_value;
   double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
-  int radius_mode = false;
+  uint8_t radius_mode = false;
   
   uint8_t absolute_override = false;          /* 1 = absolute motion for this block only {G53} */
   uint8_t next_action = NEXT_ACTION_DEFAULT;  /* The action that will be taken by the parsed line */
@@ -331,50 +313,29 @@ uint8_t gc_execute_line(char *line) {
         // 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; }        
+        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] = (x-(y*h_x2_div_d))/2;
+        offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
-        offset[gc.plane_axis_1] = (y+(x*h_x2_div_d))/2;
+        offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));
+
+      } else { // Offset mode specific computations
+
+        r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
+
       }
       
-      /*
+      // Set clockwise/counter-clockwise sign for mc_arc computations
-         This segment sets up an clockwise or counterclockwise arc from the current position to the target position around 
+      int8_t clockwise_sign = 1;
-         the center designated by the offset vector. All theta-values measured in radians of deviance from the positive 
+      if (gc.motion_mode == MOTION_MODE_CW_ARC) { clockwise_sign = -1; }
-         y-axis. 
 
-                            | <- theta == 0
-                          * * *                
-                        *       *                                               
-                      *           *                                             
-                      *     O ----T   <- theta_end (e.g. 90 degrees: theta_end == PI/2)                                          
-                      *   /                                                     
-                        C   <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
-
-      */
-            
-      // calculate the theta (angle) of the current point
-      double theta_start = theta(-offset[gc.plane_axis_0], -offset[gc.plane_axis_1]);
-      // calculate the theta (angle) of the target point
-      double theta_end = theta(target[gc.plane_axis_0] - offset[gc.plane_axis_0] - gc.position[gc.plane_axis_0], 
-         target[gc.plane_axis_1] - offset[gc.plane_axis_1] - gc.position[gc.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 (gc.motion_mode == MOTION_MODE_CCW_ARC) {
-        angular_travel = angular_travel-2*M_PI;
-      }
-      // Find the radius
-      double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]);
-      // Calculate the motion along the depth axis of the helix
-      double depth = target[gc.plane_axis_2]-gc.position[gc.plane_axis_2];
       // Trace the arc
-      mc_arc(theta_start, angular_travel, radius, depth, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2, 
+      mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
         (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
-        gc.position);
+        r, clockwise_sign);
-      // Finish off with a line to make sure we arrive exactly where we think we are
+        
-      mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], 
-        (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode);
       break;
 #endif      
     }    

motion_control.c

@@ -3,6 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Modifications Copyright (c) 2011 Sungeun (Sonny) 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
@@ -28,6 +29,8 @@
 #include "stepper.h"
 #include "planner.h"
 
+#define N_ARC_CORRECTION 25    // (0-255) Number of iterations before arc trajectory correction
+
 
 void mc_dwell(uint32_t milliseconds) 
 {
@@ -35,47 +38,113 @@ void mc_dwell(uint32_t milliseconds)
   _delay_ms(milliseconds);
 }
 
-// Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc,
+// Execute an arc in offset mode format. position == current xyz, target == target xyz, 
-// positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the
+// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is
-// circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining
+// the direction of helical travel, radius == circle radius, clockwise_sign == -1 or 1. Used
-// axis in axis_l which will be the axis for linear travel if you are tracing a helical motion.
+// for vector transformation direction.
-// position is a pointer to a vector representing the current position in millimeters.
+// position, target, and offset are pointers to vectors from gcode.c
 
 #ifdef __AVR_ATmega328P__
 // 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(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, 
+void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, 
-  int axis_linear, double feed_rate, int invert_feed_rate, double *position)
+  uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t clockwise_sign)
 {      
-  int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled();
+//   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
+//   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));
+
+  double center_axis0 = position[axis_0] + offset[axis_0];
+  double center_axis1 = position[axis_1] + offset[axis_1];
+  double linear_travel = target[axis_linear] - position[axis_linear];
+  double r_axis0 = -offset[axis_0];  // Radius vector from center to current location
+  double r_axis1 = -offset[axis_1];
+  double rt_axis0 = target[axis_0] - center_axis0;
+  double rt_axis1 = target[axis_1] - center_axis1;
+  
+  // CCW angle between position and target from circle center. Only one atan2() trig computation required.
+  double 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 (clockwise_sign < 0) { angular_travel = 2*M_PI-angular_travel; }
+  
+  double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
   if (millimeters_of_travel == 0.0) { return; }
-  uint16_t segments = round(millimeters_of_travel/settings.mm_per_arc_segment);
+  uint16_t segments = floor(millimeters_of_travel/settings.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; }
-  // The angular motion for each segment
+ 
   double theta_per_segment = angular_travel/segments;
-  // The linear motion for each segment
   double linear_per_segment = linear_travel/segments;
-  // Compute the center of this circle
+  
-  double center_x = position[axis_1]-sin(theta)*radius;
+  /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
-  double center_y = position[axis_2]-cos(theta)*radius;
+     and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
-  // a vector to track the end point of each segment
+         r_T = [cos(phi) -sin(phi);
-  double target[3];
+                sin(phi)  cos(phi] * r ;
-  int i;
+     
+     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
+  double cos_T = 1-0.5*theta_per_segment*theta_per_segment; // Small angle approximation
+  double sin_T = clockwise_sign*theta_per_segment;
+  
+  double trajectory[3];
+  double sin_Ti;
+  double cos_Ti;
+  double r_axisi;
+  uint16_t i;
+  int8_t count = 0;
+
   // Initialize the linear axis
-  target[axis_linear] = position[axis_linear];
+  trajectory[axis_linear] = position[axis_linear];
-  for (i=0; i<segments; i++) {
+
-    target[axis_linear] += linear_per_segment;
+  for (i = 1; i<segments; i++) { // Increment (segments-1)
-    theta += theta_per_segment;
+    
-    target[axis_1] = center_x+sin(theta)*radius;
+    if (count < N_ARC_CORRECTION) {
-    target[axis_2] = center_y+cos(theta)*radius;
+      // Apply vector rotation matrix 
-    plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
+      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 = clockwise_sign*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;
     }
-  plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
+
+    // Update trajectory location
+    trajectory[axis_0] = center_axis0 + r_axis0;
+    trajectory[axis_1] = center_axis1 + r_axis1;
+    trajectory[axis_linear] += linear_per_segment;
+    plan_buffer_line(trajectory[X_AXIS], trajectory[Y_AXIS], trajectory[Z_AXIS], feed_rate, invert_feed_rate);
+    
+  }
+  // Ensure last segment arrives at target location.
+  plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
+
+//   plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled);
 }
 #endif
 

motion_control.h

@@ -3,6 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Modifications Copyright (c) 2011 Sungeun (Sonny) 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
@@ -35,13 +36,12 @@
 #define mc_set_current_position(x, y, z) plan_set_current_position(x, y, z) 
 
 #ifdef __AVR_ATmega328P__
-// Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc,
+// Execute an arc in offset mode format. position == current xyz, target == target xyz, 
-// positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the
+// offset == offset from current xyz, axis_XXX defines circle plane in tool space, axis_linear is
-// circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining
+// the direction of helical travel, radius == circle radius, clockwise_sign == -1 or 1. Used
-// axis in axis_l which will be the axis for linear travel if you are tracing a helical motion.
+// for vector transformation direction.
-
+void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1,
-void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, 
+  uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t clockwise_sign);
-  int axis_linear, double feed_rate, int invert_feed_rate, double *position);
 #endif
   
 // Dwell for a couple of time units

nuts_bolts.h

@@ -3,6 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Modifications Copyright (c) 2011 Sungeun (Sonny) 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

planner.c

@@ -3,7 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
-  Modifications Copyright (c) 2011 Sungeun Jeon
+  Modifications Copyright (c) 2011 Sungeun (Sonny) 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
@@ -90,10 +90,11 @@ static double intersection_distance(double initial_rate, double final_rate, doub
 }
 
             
-// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the 
+// Calculates the square of the maximum allowable speed at this point when you must be able to reach 
-// acceleration within the allotted distance.
+// target_velocity using the acceleration within the allotted distance.
-static double max_allowable_speed(double acceleration, double target_velocity, double distance) {
+// NOTE: sqrt() removed for speed optimization. Related calculations in terms of square velocity.
-  return( sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance) );
+static double max_allowable_speed_sqr(double acceleration, double target_velocity_sqr, double distance) {
+  return( target_velocity_sqr-2*acceleration*60*60*distance );
 }
 
 
@@ -101,19 +102,29 @@ static double max_allowable_speed(double acceleration, double target_velocity, d
 static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
   if (!current) { return; }
   
-  double entry_speed = current->max_entry_speed; // Re-write to ensure at max possible speed
+  double entry_speed_sqr = current->max_entry_speed_sqr; // Reset and check to ensure max possible speed
-  double exit_speed;
+    
+  // If nominal length true, nominal speed is guaranteed to be reached. No need to re-compute.
+  // But, if forward planner changed entry speed, reset to max entry speed just to be sure.
+  if (!current->nominal_length_flag) {
     if (next) {
-    exit_speed = next->entry_speed;
+      // If the required deceleration across the block is too rapid, reduce entry_speed_sqr accordingly.
+      if (entry_speed_sqr > next->entry_speed_sqr) {
+        entry_speed_sqr = min( entry_speed_sqr,
+          max_allowable_speed_sqr(-settings.acceleration,next->entry_speed_sqr,current->millimeters));
+      }
     } else { 
-    exit_speed = 0.0; // Assume last block has zero exit velocity
+      // Assume last block has zero exit velocity.
+      entry_speed_sqr = min( entry_speed_sqr,
+          max_allowable_speed_sqr(-settings.acceleration,0.0,current->millimeters));
     }
-  // If the required deceleration across the block is too rapid, reduce the entry_speed accordingly.
-  if (entry_speed > exit_speed) {
-    entry_speed = 
-      min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed);
   }
-  current->entry_speed = entry_speed;
+
+  // Check for junction speed change
+  if (current->entry_speed_sqr != entry_speed_sqr) {
+    current->entry_speed_sqr = entry_speed_sqr;
+    current->recalculate_flag = true; // Note: Newest block already set to true
+  }
 }
 
 
@@ -136,13 +147,26 @@ static void planner_reverse_pass() {
 // The kernel called by planner_recalculate() when scanning the plan from first to last entry.
 static void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
   if(!current) { return; }
+
+  if(previous) {
+
+    // If nominal length true, nominal speed is guaranteed to be reached. No need to recalculate.  
+    if (!previous->nominal_length_flag) {
       // If the previous block is an acceleration block, but it is not long enough to 
       // complete the full speed change within the block, we need to adjust the entry
       // speed accordingly.
-  if(previous) {
+      if (previous->entry_speed_sqr < current->entry_speed_sqr) {
-    if (previous->entry_speed < current->entry_speed) {
+        double entry_speed_sqr = min( current->entry_speed_sqr, current->max_entry_speed_sqr );
-      current->entry_speed = min( min( current->entry_speed, current->max_entry_speed ), 
+        entry_speed_sqr = min( entry_speed_sqr,
-        max_allowable_speed(-settings.acceleration,previous->entry_speed,previous->millimeters) );
+          max_allowable_speed_sqr(-settings.acceleration,previous->entry_speed_sqr,previous->millimeters) );
+
+        // Check for junction speed change
+        if (current->entry_speed_sqr != entry_speed_sqr) {
+          current->entry_speed_sqr = entry_speed_sqr;
+          current->recalculate_flag = true;
+        }
+      }
+      
     }
   }
 }
@@ -165,7 +189,7 @@ static void planner_forward_pass() {
 }
 
 
-/*                                                                              
+/*                             STEPPER RATE DEFINITION                                              
                                      +--------+   <- nominal_rate
                                     /          \                                
     nominal_rate*entry_factor ->   +            \                               
@@ -175,6 +199,7 @@ static void planner_forward_pass() {
 */                                                                              
 // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
 // The factors represent a factor of braking and must be in the range 0.0-1.0.
+// This converts the planner parameters to the data required by the stepper controller.
 static void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) {
   
   block->initial_rate = ceil(block->nominal_rate*entry_factor);
@@ -201,10 +226,19 @@ static void calculate_trapezoid_for_block(block_t *block, double entry_factor, d
   block->decelerate_after = accelerate_steps+plateau_steps;
 }     
 
-
+/*                            PLANNER SPEED DEFINITION                                              
-// Recalculates the trapezoid speed profiles for all blocks in the plan according to the 
+                                     +--------+   <- current->nominal_speed
-// entry_speed for each junction. Must be called by planner_recalculate() after 
+                                    /          \                                
-// updating the blocks.
+         current->entry_speed ->   +            \                               
+                                   |             + <- next->entry_speed
+                                   +-------------+                              
+                                       time -->                                 
+*/                                                                              
+// Recalculates the trapezoid speed profiles for flagged blocks in the plan according to the 
+// entry_speed for each junction and the entry_speed of the next junction. Must be called by 
+// planner_recalculate() after updating the blocks. Any recalulate flagged junction will
+// compute the two adjacent trapezoids to the junction, since the junction speed corresponds 
+// to exit speed and entry speed of one another.
 static void planner_recalculate_trapezoids() {
   int8_t block_index = block_buffer_tail;
   block_t *current;
@@ -214,21 +248,28 @@ static void planner_recalculate_trapezoids() {
     current = next;
     next = &block_buffer[block_index];
     if (current) {
-      // Compute entry and exit factors for trapezoid calculations
+      // Recalculate if current block entry or exit junction speed has changed.
-      double entry_factor = current->entry_speed/current->nominal_speed;
+      if (current->recalculate_flag || next->recalculate_flag) {
-      double exit_factor = next->entry_speed/current->nominal_speed;
+        // Compute entry and exit factors for trapezoid calculations. 
+        // NOTE: sqrt(square velocities) now performed only when required in trapezoid calculation.
+        double entry_factor = sqrt( current->entry_speed_sqr ) / current->nominal_speed;
+        double exit_factor = sqrt( next->entry_speed_sqr ) / current->nominal_speed;
         calculate_trapezoid_for_block(current, entry_factor, exit_factor);      
+        current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
+      }
     }
     block_index = next_block_index( block_index );
   }
-  calculate_trapezoid_for_block(next, next->entry_speed, 0.0); // Last block
+  // Last/newest block in buffer. Exit speed is zero.
+  calculate_trapezoid_for_block(next, sqrt( next->entry_speed_sqr ) / next->nominal_speed, 0.0);
+  next->recalculate_flag = false;
 }
 
 // Recalculates the motion plan according to the following algorithm:
 //
 //   1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed) 
 //      so that:
-//     a. The maximum junction speed is within the set limit
+//     a. The junction speed is equal to or less than the maximum junction speed limit
 //     b. No speed reduction within one block requires faster deceleration than the one, true constant 
 //        acceleration.
 //   2. Go over every block in chronological order and dial down junction speed values if 
@@ -237,9 +278,10 @@ static void planner_recalculate_trapezoids() {
 //
 // When these stages are complete all blocks have an entry speed that will allow all speed changes to 
 // be performed using only the one, true constant acceleration, and where no junction speed is greater
-// than the set limit. Finally it will:
+// than the max limit. Finally it will:
 //
-//   3. Recalculate trapezoids for all blocks using the recently updated junction speeds.
+//   3. Recalculate trapezoids for all blocks using the recently updated junction speeds. Block trapezoids
+//      with no updated junction speeds will not be recalculated and assumed ok as is.
 
 static void planner_recalculate() {     
   planner_reverse_pass();
@@ -256,7 +298,7 @@ void plan_init() {
   previous_nominal_speed = 0.0;
 }
 
-void plan_set_acceleration_manager_enabled(int enabled) {
+void plan_set_acceleration_manager_enabled(uint8_t enabled) {
   if ((!!acceleration_manager_enabled) != (!!enabled)) {
     st_synchronize();
     acceleration_manager_enabled = !!enabled;
@@ -282,8 +324,7 @@ block_t *plan_get_current_block() {
 // Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
 // millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
 // rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
-void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate) {
+void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate) {
-  // The target position of the tool in absolute steps
   
   // Calculate target position in absolute steps
   int32_t target[3];
@@ -299,6 +340,13 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
   
   // Prepare to set up new block
   block_t *block = &block_buffer[block_buffer_head];
+
+  // Compute direction bits for this block
+  block->direction_bits = 0;
+  if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
+  if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
+  if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
+  
   // Number of steps for each axis
   block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
   block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
@@ -308,7 +356,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
   // Bail if this is a zero-length block
   if (block->step_event_count == 0) { return; };
   
-  // Compute path vector in terms of quantized step target and current positions
+  // Compute path vector in terms of absolute step target and current positions
   double delta_mm[3];
   delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS];
   delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS];
@@ -341,53 +389,68 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
   // 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 = block->millimeters/block->step_event_count;
+  double step_per_travel = block->step_event_count/block->millimeters; // Compute inverse to remove divide
-  block->rate_delta = ceil(
+  block->rate_delta = step_per_travel * ceil(                        // convert to: acceleration steps/min/acceleration_tick    
-    ((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick
+    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    
 
+  // Perform planner-enabled calculations
   if (acceleration_manager_enabled) {  
   
     // Compute path unit vector                            
     double unit_vec[3];
-    unit_vec[X_AXIS] = delta_mm[X_AXIS]/block->millimeters;
+    double inv_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides
-    unit_vec[Y_AXIS] = delta_mm[Y_AXIS]/block->millimeters;
+    unit_vec[X_AXIS] = delta_mm[X_AXIS]*inv_millimeters;
-    unit_vec[Z_AXIS] = delta_mm[Z_AXIS]/block->millimeters;  
+    unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inv_millimeters;
+    unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inv_millimeters;  
   
     // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
+    // Does not actually deviate from path, but used as a robust way to compute cornering speeds.
     // Let a circle be tangent to both previous and current path line segments, where the junction 
-    // deviation is defined as the distance from the junction to the edge of the circle. The
+    // deviation is defined as the distance from the junction to the closest edge of the circle, 
-    // circular segment joining the two paths represents the path of centripetal acceleration.
+    // colinear with the circle center. The circular segment joining the two paths represents the 
-    // Solve for max velocity based on max acceleration about the radius of the circle, defined 
+    // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
-    // indirectly by junction deviation, which may be also viewed as path width or max_jerk.
+    // radius of the circle, defined indirectly by junction deviation. This may be also viewed as 
-    double vmax_junction = 0.0; // Set default zero max junction speed
+    // path width or max_jerk in the previous grbl version.
+    // NOTE: sqrt() removed for speed optimization. Related calculations in terms of square velocity.
 
-    // Use default for first block or when planner is reset by previous_nominal_speed = 0.0
+    double vmax_junction_sqr = 0.0; // Set default zero max junction speed
+
+    // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
     if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
-      // Compute cosine of angle between previous and current path
+      // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
-      double cos_theta = ( -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] + 
+      // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
-                           -previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] + 
+      double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] 
-                           -previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] );
+                         - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] 
+                         - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
                            
-      // Avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
+      // Skip and use default zero max junction speed for 0 degree acute junction.
-      vmax_junction = min(previous_nominal_speed,block->nominal_speed);
+      if (cos_theta < 1.0) {
+        vmax_junction_sqr = square( min(previous_nominal_speed,block->nominal_speed) );
+        // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
         if (cos_theta > -1.0) {
           // Compute maximum junction velocity based on maximum acceleration and junction deviation
-        double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity
+          double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
-        vmax_junction = max(0.0, min(vmax_junction,
+          vmax_junction_sqr = min(vmax_junction_sqr,
-          sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ) );
+            settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2) );
         }
       }
+    }
+    block->max_entry_speed_sqr = vmax_junction_sqr;
+    block->entry_speed_sqr = vmax_junction_sqr;
 
-    block->max_entry_speed = vmax_junction;
+    // Initialize planner efficiency flags
-    block->entry_speed = vmax_junction;
+    // Set flag if block will always reach nominal speed regardless of entry/exit speeds.
+    if (block->nominal_speed <= sqrt(max_allowable_speed_sqr(-settings.acceleration,0.0,0.5*block->millimeters)) ) 
+      { block->nominal_length_flag = true; }
+    else { block->nominal_length_flag = false; }
+    block->recalculate_flag = true; // Always calculate trapezoid for new block
   
     // Update previous path unit_vector and nominal speed
     memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[]
     previous_nominal_speed = block->nominal_speed;
 
   } else {
-    // Set at nominal rates only for disabled acceleration planner
+    // Acceleration planner disabled. Set minimum that is required.
     block->initial_rate = block->nominal_rate;
     block->final_rate = block->nominal_rate;
     block->accelerate_until = 0;
@@ -395,12 +458,6 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
     block->rate_delta = 0;
   }
   
-  // Compute direction bits for this block
-  block->direction_bits = 0;
-  if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
-  if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
-  if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
-  
   // Move buffer head
   block_buffer_head = next_buffer_head;     
   // Update position

planner.h

@@ -3,6 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Modifications Copyright (c) 2011 Sungeun (Sonny) 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
@@ -35,9 +36,11 @@ typedef struct {
   // Fields used by the motion planner to manage acceleration
   double speed_x, speed_y, speed_z;   // Nominal mm/minute for each axis
   double nominal_speed;               // The nominal speed for this block in mm/min  
+  double entry_speed_sqr;             // Square of entry speed at previous-current junction in (mm/min)^2
+  double max_entry_speed_sqr;         // Square of maximum allowable entry speed in (mm/min)^2
   double millimeters;                 // The total travel of this block in mm
-  double entry_speed;                 // Entry speed at previous-current junction 
+  uint8_t recalculate_flag;           // Planner flag to recalculate trapezoids on entry junction
-  double max_entry_speed;             // Maximum allowable entry speed
+  uint8_t nominal_length_flag;        // Planner flag for nominal speed always reached
 
   // Settings for the trapezoid generator
   uint32_t initial_rate;              // The jerk-adjusted step rate at start of block  
@@ -54,7 +57,7 @@ void plan_init();
 // Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
 // millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
 // rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
-void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate);
+void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate);
 
 // Called when the current block is no longer needed. Discards the block and makes the memory
 // availible for new blocks.
@@ -64,7 +67,7 @@ void plan_discard_current_block();
 block_t *plan_get_current_block();
 
 // Enables or disables acceleration-management for upcoming blocks
-void plan_set_acceleration_manager_enabled(int enabled);
+void plan_set_acceleration_manager_enabled(uint8_t enabled);
 
 // Is acceleration-management currently enabled?
 int plan_is_acceleration_manager_enabled();

protocol.c

@@ -3,6 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Modifications Copyright (c) 2011 Sungeun (Sonny) 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

script/grbl_preprocess.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python
 """\
-G-code preprocessor for grbl
+G-code preprocessor for grbl (BETA!)
 - Converts G02/03 arcs to G01 linear interpolations
 - Removes comments, block delete characters, and line numbers
 - Removes spaces and capitalizes commands
@@ -16,6 +16,7 @@ TODO:
 
 Based on GRBL 0.7b source code by Simen Svale Skogsrud
 
+By: Sungeun (Sonny) Jeon
 Version: 20100825
 """
 import re

settings.c

@@ -3,6 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Modifications Copyright (c) 2011 Sungeun (Sonny) 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

settings.h

@@ -3,6 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Modifications Copyright (c) 2011 Sungeun (Sonny) 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