Further planner improvements and misc minor bug fixes. Memory savings and increased buffer size.

- Update grbl version and settings version to automatically reset
eeprom. FYI, this will reset your grbl settings. - Saved
3*BLOCK_BUFFER_SIZE doubles in static memory by removing obsolete
variables: speed_x, speed_y, and speed_z. - Increased buffer size
conservatively to 18 from 16. (Probably can do 20). - Removed expensive!
modulo operator from block indexing function. Reduces significant
computational overhead. - Re-organized some sqrt() calls to be more
efficient during time critical planning cases, rather than non-time
critical. - Minor bug fix in planner max junction velocity logic. -
Simplified arc logic and removed need to multiply for CW or CCW
direction.

gcode.c

@@ -3,7 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
-  Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon
+  Copyright (c) 2011 Sungeun K. 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
@@ -328,13 +328,13 @@ uint8_t gc_execute_line(char *line) {
       }
       
       // Set clockwise/counter-clockwise sign for mc_arc computations
-      int8_t clockwise_sign = 1;
+      int8_t isclockwise = false;
-      if (gc.motion_mode == MOTION_MODE_CW_ARC) { clockwise_sign = -1; }
+      if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
 
       // Trace the arc
       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,
-        r, clockwise_sign);
+        r, isclockwise);
         
       break;
 #endif      

motion_control.c

@@ -3,7 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
-  Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon
+  Copyright (c) 2011 Sungeun K. 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
@@ -48,7 +48,7 @@ void mc_dwell(uint32_t milliseconds)
 // 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 *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1, 
-  uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t clockwise_sign)
+  uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t isclockwise)
 {      
 //   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
@@ -64,7 +64,7 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui
   // 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; }
+  if (isclockwise) { angular_travel -= 2*M_PI; }
   
   double millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));
   if (millimeters_of_travel == 0.0) { return; }
@@ -104,7 +104,7 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui
   */
   // 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 sin_T = theta_per_segment;
   
   double trajectory[3];
   double sin_Ti;
@@ -128,7 +128,7 @@ void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, ui
       // 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);
+      sin_Ti = 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;

motion_control.h

@@ -3,7 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
-  Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon
+  Copyright (c) 2011 Sungeun K. 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
@@ -41,7 +41,7 @@
 // the direction of helical travel, radius == circle radius, clockwise_sign == -1 or 1. Used
 // for vector transformation direction.
 void mc_arc(double *position, double *target, double *offset, uint8_t axis_0, uint8_t axis_1,
-  uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t clockwise_sign);
+  uint8_t axis_linear, double feed_rate, uint8_t invert_feed_rate, double radius, int8_t isclockwise);
 #endif
   
 // Dwell for a couple of time units

nuts_bolts.h

@@ -3,7 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
-  Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon  
+  Copyright (c) 2011 Sungeun K. 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 (Sonny) Jeon
+  Copyright (c) 2011 Sungeun K. 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
@@ -33,7 +33,7 @@
 
 // The number of linear motions that can be in the plan at any give time
 #ifdef __AVR_ATmega328P__
-#define BLOCK_BUFFER_SIZE 16
+#define BLOCK_BUFFER_SIZE 18
 #else
 #define BLOCK_BUFFER_SIZE 5
 #endif
@@ -52,15 +52,18 @@ static uint8_t acceleration_manager_enabled;   // Acceleration management active
 
 
 // Returns the index of the next block in the ring buffer
+// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
 static int8_t next_block_index(int8_t block_index) {
-  return( (block_index + 1) % BLOCK_BUFFER_SIZE );
+  block_index++;
+  if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
+  return(block_index);
 }
 
 
 // Returns the index of the previous block in the ring buffer
 static int8_t prev_block_index(int8_t block_index) {
-  block_index--;
+  if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE-1; }
-  if (block_index < 0) { block_index = BLOCK_BUFFER_SIZE-1; }
+  else { block_index--; }
   return(block_index);
 }
 
@@ -90,41 +93,38 @@ static double intersection_distance(double initial_rate, double final_rate, doub
 }
 
             
-// Calculates the square of the maximum allowable speed at this point when you must be able to reach 
+// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity
-// target_velocity using the acceleration within the allotted distance.
+// using the acceleration within the allotted distance.
-// NOTE: sqrt() removed for speed optimization. Related calculations in terms of square velocity.
+// NOTE: sqrt() reimplimented here from prior version due to improved planner logic. Increases speed
-static double max_allowable_speed_sqr(double acceleration, double target_velocity_sqr, double distance) {
+// in time critical computations, i.e. arcs or rapid short lines from curves. Guaranteed to not exceed
-  return( target_velocity_sqr-2*acceleration*60*60*distance );
+// BLOCK_BUFFER_SIZE calls per planner cycle.
+static double max_allowable_speed(double acceleration, double target_velocity, double distance) {
+  return( sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance) );
 }
 
 
 // The kernel called by planner_recalculate() when scanning the plan from last to first entry.
 static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
-  if (!current) { return; }
+  if (!current) { return; }  // Cannot operate on nothing.
   
-  double entry_speed_sqr = current->max_entry_speed_sqr; // Reset and check to ensure max possible speed
+  if (next) { 
+    // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
+    // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and 
+    // check for maximum allowable speed reductions to ensure maximum possible planned speed.
+    if (current->entry_speed != current->max_entry_speed) {
     
-  // If nominal length true, nominal speed is guaranteed to be reached. No need to re-compute.
+      // If nominal length true, max junction speed is guaranteed to be reached. Only compute
-  // But, if forward planner changed entry speed, reset to max entry speed just to be sure.
+      // for max allowable speed if block is decelerating and nominal length is false.
-  if (!current->nominal_length_flag) {
+      if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) {
-    if (next) {
+        current->entry_speed = min( current->max_entry_speed,
-      // If the required deceleration across the block is too rapid, reduce entry_speed_sqr accordingly.
+          max_allowable_speed(-settings.acceleration,next->entry_speed,current->millimeters));
-      if (entry_speed_sqr > next->entry_speed_sqr) {
+      } else {
-        entry_speed_sqr = min( entry_speed_sqr,
+        current->entry_speed = current->max_entry_speed;
-          max_allowable_speed_sqr(-settings.acceleration,next->entry_speed_sqr,current->millimeters));
       } 
-    } else { 
+      current->recalculate_flag = true;    
-      // Assume last block has zero exit velocity.
-      entry_speed_sqr = min( entry_speed_sqr,
-          max_allowable_speed_sqr(-settings.acceleration,0.0,current->millimeters));
-    }
-  }
     
-  // Check for junction speed change
+    }
-  if (current->entry_speed_sqr != entry_speed_sqr) {
+  } // Skip last block. Already initialized and set for recalculation.
-    current->entry_speed_sqr = entry_speed_sqr;
-    current->recalculate_flag = true; // Note: Newest block already set to true
-  }
 }
 
 
@@ -140,33 +140,28 @@ static void planner_reverse_pass() {
     block[0] = &block_buffer[block_index];
     planner_reverse_pass_kernel(block[0], block[1], block[2]);
   }
-  // Skip buffer tail to prevent over-writing the initial entry speed.
+  // Skip buffer tail/first block to prevent over-writing the initial entry speed.
 }
 
 
 // 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) { return; }  // Begin planning after buffer_tail
   
-  if(previous) {
+  // 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. Entry
+  // speeds have already been reset, maximized, and reverse planned by reverse planner.
+  // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.  
+  if (!previous->nominal_length_flag) {
+    if (previous->entry_speed < current->entry_speed) {
+      double entry_speed = min( current->entry_speed,
+        max_allowable_speed(-settings.acceleration,previous->entry_speed,previous->millimeters) );
 
-    // If nominal length true, nominal speed is guaranteed to be reached. No need to recalculate.  
+      // Check for junction speed change
-    if (!previous->nominal_length_flag) {
+      if (current->entry_speed != entry_speed) {
-      // If the previous block is an acceleration block, but it is not long enough to 
+        current->entry_speed = entry_speed;
-      // complete the full speed change within the block, we need to adjust the entry
+        current->recalculate_flag = true;
-      // speed accordingly.
-      if (previous->entry_speed_sqr < current->entry_speed_sqr) {
-        double entry_speed_sqr = min( current->entry_speed_sqr, current->max_entry_speed_sqr );
-        entry_speed_sqr = min( entry_speed_sqr,
-          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;
-        }
       }
-      
     }    
   }
 }
@@ -250,18 +245,16 @@ static void planner_recalculate_trapezoids() {
     if (current) {
       // Recalculate if current block entry or exit junction speed has changed.
       if (current->recalculate_flag || next->recalculate_flag) {
-        // Compute entry and exit factors for trapezoid calculations. 
+        // NOTE: Entry and exit factors always > 0 by all previous logic operations.     
-        // NOTE: sqrt(square velocities) now performed only when required in trapezoid calculation.
+        calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed,
-        double entry_factor = sqrt( current->entry_speed_sqr ) / current->nominal_speed;
+          next->entry_speed/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 );
   }
-  // Last/newest block in buffer. Exit speed is zero.
+  // Last/newest block in buffer. Exit speed is zero. Always recalculated.
-  calculate_trapezoid_for_block(next, sqrt( next->entry_speed_sqr ) / next->nominal_speed, 0.0);
+  calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed, 0.0);
   next->recalculate_flag = false;
 }
 
@@ -373,9 +366,6 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
   
   // Calculate speed in mm/minute for each axis
   double multiplier = 60.0*1000000.0/microseconds;
-  block->speed_x = delta_mm[X_AXIS] * multiplier;
-  block->speed_y = delta_mm[Y_AXIS] * multiplier;
-  block->speed_z = delta_mm[Z_AXIS] * multiplier; 
   block->nominal_speed = block->millimeters * multiplier;
   block->nominal_rate = ceil(block->step_event_count * multiplier);  
   
@@ -404,16 +394,15 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
     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 closest edge of the circle, 
     // colinear with the circle center. The circular segment joining the two paths represents the 
     // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
     // radius of the circle, defined indirectly by junction deviation. This may be also viewed as 
-    // path width or max_jerk in the previous grbl version.
+    // path width or max_jerk in the previous grbl version. This approach does not actually deviate 
-    // NOTE: sqrt() removed for speed optimization. Related calculations in terms of square velocity.
+    // from path, but used as a robust way to compute cornering speeds, as it takes into account the
-
+    // nonlinearities of both the junction angle and junction velocity.
-    double vmax_junction_sqr = 0.0; // Set default zero max junction speed
+    double vmax_junction = 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)) {
@@ -423,25 +412,33 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t in
                          - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] 
                          - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
                            
-      // Skip and use default zero max junction speed for 0 degree acute junction.
+      // Skip and use default max junction speed for 0 degree acute junction.
-      if (cos_theta < 1.0) {
+      if (cos_theta < 0.95) {
-        vmax_junction_sqr = square( min(previous_nominal_speed,block->nominal_speed) );
+        vmax_junction = 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) {
+        if (cos_theta > -0.95) {
           // 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. Always positive.
-          vmax_junction_sqr = min(vmax_junction_sqr,
+          vmax_junction = min(vmax_junction,
-            settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2) );
+            sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
         }
       }
     }
-    block->max_entry_speed_sqr = vmax_junction_sqr;
+    block->max_entry_speed = vmax_junction;
-    block->entry_speed_sqr = vmax_junction_sqr;
+    
+    // Initialize block entry speed. Compute based on deceleration to rest (zero speed).
+    double v_allowable = max_allowable_speed(-settings.acceleration,0.0,block->millimeters);
+    block->entry_speed = min(vmax_junction, v_allowable);
 
     // Initialize planner efficiency flags
-    // Set flag if block will always reach nominal speed regardless of entry/exit speeds.
+    // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
-    if (block->nominal_speed <= sqrt(max_allowable_speed_sqr(-settings.acceleration,0.0,0.5*block->millimeters)) ) 
+    // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
-      { block->nominal_length_flag = true; }
+    // the current block and next block junction speeds are guaranteed to always be at their maximum
+    // junction speeds in deceleration and acceleration, respectively. This is due to how the current
+    // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
+    // the reverse and forward planners, the corresponding block junction speed will always be at the
+    // the maximum junction speed and may always be ignored for any speed reduction checks.
+    if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
     else { block->nominal_length_flag = false; }
     block->recalculate_flag = true; // Always calculate trapezoid for new block
   
@@ -472,5 +469,6 @@ void plan_set_current_position(double x, double y, double z) {
   position[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]);
   position[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]);
   position[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);    
-  previous_nominal_speed = 0.0;
+  previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
+  clear_vector_double(previous_unit_vec);
 }

planner.h

@@ -3,7 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
-  Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon  
+  Copyright (c) 2011 Sungeun K. 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
@@ -34,10 +34,9 @@ typedef struct {
   uint32_t nominal_rate;              // The nominal step rate for this block in step_events/minute
   
   // 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 entry_speed;                 // Entry speed at previous-current junction in mm/min
-  double max_entry_speed_sqr;         // Square of maximum allowable entry speed in (mm/min)^2
+  double max_entry_speed;             // Maximum allowable junction entry speed in mm/min
   double millimeters;                 // The total travel of this block in mm
   uint8_t recalculate_flag;           // Planner flag to recalculate trapezoids on entry junction
   uint8_t nominal_length_flag;        // Planner flag for nominal speed always reached

protocol.c

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

@@ -8,6 +8,8 @@ G-code preprocessor for grbl (BETA!)
 - OPTIONAL: Remove unsupported grbl G and M commands
 
 TODO: 
+- Number precision truncation
+- Arc conversion option
 - More robust error checking
 - Improve interface to command line options
 - Improve g-code parsing to NIST standards

settings.c

@@ -3,7 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
-  Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon  
+  Copyright (c) 2011 Sungeun 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
@@ -52,7 +52,7 @@ typedef struct {
 #define DEFAULT_RAPID_FEEDRATE 500.0 // in millimeters per minute
 #define DEFAULT_FEEDRATE 500.0
 #define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/10.0)
-#define DEFAULT_JUNCTION_DEVIATION 0.1
+#define DEFAULT_JUNCTION_DEVIATION 0.05
 #define DEFAULT_STEPPING_INVERT_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT))
 
 void settings_reset() {

settings.h

@@ -3,7 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
-  Modifications Copyright (c) 2011 Sungeun (Sonny) Jeon
+  Copyright (c) 2011 Sungeun K. 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
@@ -26,11 +26,11 @@
 #include <math.h>
 #include <inttypes.h>
 
-#define GRBL_VERSION "0.7b"
+#define GRBL_VERSION "0.7c"
 
 // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl
 // when firmware is upgraded. Always stored in byte 0 of eeprom
-#define SETTINGS_VERSION 2
+#define SETTINGS_VERSION 3
 
 // Current global settings (persisted in EEPROM from byte 1 onwards)
 typedef struct {