Planner execution time halved and bug fixes. Increased step rate limit to 30kHz.

- Planner execute speed has been more than halved from 4ms to 1.9ms
when computing a plan for a single line segment during arc generation.
This means that Grbl can now run through an arc (or complex curve)
twice as fast as before without starving the buffer. For 0.1mm arc
segments, this means about the theoretical feed rate limit is about
3000mm/min for arcs now.

- Increased the Ranade timer frequency to 30kHz, as there doesn't seem
to be any problems with increasing the frequency. This means that the
maximum step frequency is now back at 30kHz.

- Added Zen Toolworks 7x7 defaults.

config.h

@@ -25,7 +25,7 @@
 // IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them.
 
 // Default settings. Used when resetting EEPROM. Change to desired name in defaults.h
-#define DEFAULTS_SHERLINE_5400
+#define DEFAULTS_GENERIC
 
 // Serial baud rate
 #define BAUD_RATE 9600
@@ -122,7 +122,7 @@
 // frequency goes up. So there will be little left for other processes like arcs.  
 //   In future versions, more work will be done to increase the step rates but still stay around
 // 20kHz by performing two steps per step event, rather than just one.
-#define ISR_TICKS_PER_SECOND 20000L  // Integer (Hz)
+#define ISR_TICKS_PER_SECOND 30000L  // Integer (Hz)
 
 // The Ranade algorithm can use either floating point or long integers for its counters, but for 
 // integers the counter values must be scaled since these values can be very small (10^-6). This
@@ -143,6 +143,7 @@
 // in the stepper program and never runs slower than this value. If the RANADE_MULTIPLIER value
 // changes, it will affect how this value works. So, if a zero is add/subtracted from the
 // RANADE_MULTIPLIER value, do the same to this value if you want to same response. 
+// NOTE: Compute by (desired_step_rate/60) * RANADE_MULTIPLIER/ISR_TICKS_PER_SECOND. (mm/min)
 #define MINIMUM_STEP_RATE 1000L // Integer (mult*mm/isr_tic)
 
 // Minimum stepper rate. Only used by homing at this point. May be removed in later releases.

defaults.h

@@ -55,8 +55,8 @@
 #endif
 
 #ifdef DEFAULTS_SHERLINE_5400
-  // Description: Sherline 5400 mill with three NEMA 23 185 oz-in stepper motors, driven by
+  // Description: Sherline 5400 mill with three NEMA 23 Keling  KL23H256-21-8B 185 oz-in stepper motors,
-  // three Pololu A4988 stepper drivers with a 30V, 6A power supply at 1.5A per winding.
+  // driven by three Pololu A4988 stepper drivers with a 30V, 6A power supply at 1.5A per winding.
   #define MICROSTEPS 2
   #define STEPS_PER_REV 200.0
   #define MM_PER_REV (0.050*MM_PER_INCH) // 0.050 inch/rev leadscrew
@@ -119,4 +119,36 @@
   #define DEFAULT_N_ARC_CORRECTION 25
 #endif
 
+#ifdef DEFAULTS_ZEN_TOOLWORKS_7x7
+  // Description: Zen Toolworks 7x7 mill with three Shinano SST43D2121 65oz-in NEMA 17 stepper motors.
+  // Leadscrew is different from some ZTW kits, where most are 1.25mm/rev rather than 8.0mm/rev here.
+  // Driven by 30V, 6A power supply and TI DRV8811 stepper motor drivers.
+  #define MICROSTEPS 8
+  #define STEPS_PER_REV 200.0
+  #define MM_PER_REV 8.0 // 8 mm/rev leadscrew
+  #define DEFAULT_X_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV)
+  #define DEFAULT_Y_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV)
+  #define DEFAULT_Z_STEPS_PER_MM (STEPS_PER_REV*MICROSTEPS/MM_PER_REV)
+  #define DEFAULT_STEP_PULSE_MICROSECONDS 10
+  #define DEFAULT_MM_PER_ARC_SEGMENT 0.1
+  #define DEFAULT_RAPID_FEEDRATE 2500.0 // mm/min
+  #define DEFAULT_FEEDRATE 1000.0 // mm/min
+  #define DEFAULT_ACCELERATION 150.0*60*60 // 150 mm/min^2
+  #define DEFAULT_JUNCTION_DEVIATION 0.05 // mm
+  #define DEFAULT_STEPPING_INVERT_MASK (1<<Y_DIRECTION_BIT)
+  #define DEFAULT_REPORT_INCHES 0 // false
+  #define DEFAULT_AUTO_START 1 // true
+  #define DEFAULT_INVERT_ST_ENABLE 0 // false
+  #define DEFAULT_HARD_LIMIT_ENABLE 0  // false
+  #define DEFAULT_HOMING_ENABLE 0  // false
+  #define DEFAULT_HOMING_DIR_MASK 0 // move positive dir
+  #define DEFAULT_HOMING_RAPID_FEEDRATE 500.0 // mm/min
+  #define DEFAULT_HOMING_FEEDRATE 50.0 // mm/min
+  #define DEFAULT_HOMING_DEBOUNCE_DELAY 100 // msec (0-65k)
+  #define DEFAULT_HOMING_PULLOFF 1.0 // mm
+  #define DEFAULT_STEPPER_IDLE_LOCK_TIME 25 // msec (0-255)
+  #define DEFAULT_DECIMAL_PLACES 3
+  #define DEFAULT_N_ARC_CORRECTION 25
+#endif
+
 #endif

planner.c

@@ -38,10 +38,10 @@ static uint8_t next_buffer_head;                 // Index of the next buffer hea
 
 // Define planner variables
 typedef struct {
-  int32_t position[3];             // The planner position of the tool in absolute steps. Kept separate
+  int32_t position[N_AXIS];        // The planner position of the tool in absolute steps. Kept separate
                                    // from g-code position for movements requiring multiple line motions,
                                    // i.e. arcs, canned cycles, and backlash compensation.
-  float previous_unit_vec[3];     // Unit vector of previous path line segment
+  float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment
   float previous_nominal_speed;    // Nominal speed of previous path line segment
 } planner_t;
 static planner_t pl;
@@ -65,12 +65,11 @@ static uint8_t prev_block_index(uint8_t block_index)
 }
 
 
-// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity
+// Calculates the final velocity from an initial velocity over a distance with constant acceleration.
-// using the acceleration within the allotted distance.
 // NOTE: Guaranteed to not exceed BLOCK_BUFFER_SIZE calls per planner cycle.
-static float max_allowable_speed(float acceleration, float target_velocity, float distance) 
+static float calculate_final_velocity(float acceleration, float initial_velocity, float distance) 
 {
-  return( sqrt(target_velocity*target_velocity-2*acceleration*distance) );
+  return( sqrt(initial_velocity*initial_velocity+2*acceleration*distance) );
 }
 
 
@@ -94,7 +93,7 @@ static float max_allowable_speed(float acceleration, float target_velocity, floa
   
   The following function determines the type of velocity profile and stores the minimum required
   information for the stepper algorithm to execute the calculated profiles. In this case, only
-  the new initial rate and steps until deceleration are computed, since the stepper algorithm
+  the new initial rate and n_steps until deceleration are computed, since the stepper algorithm
   already handles acceleration and cruising and just needs to know when to start decelerating.
 */
 static void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed) 
@@ -102,20 +101,24 @@ static void calculate_trapezoid_for_block(block_t *block, float entry_speed, flo
   // Compute new initial rate for stepper algorithm
   block->initial_rate = ceil(entry_speed*(RANADE_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
     
-  // First determine intersection distance from the exit point for a triangular profile.
+  // Compute efficiency variable for following calculations. Removes a float divide and multiply.
-  float steps_per_mm = block->step_event_count/block->millimeters;
+  // TODO: If memory allows, this can be kept in the block buffer since it doesn't change, even after feed holds.
-  int32_t intersect_distance = ceil( steps_per_mm * 
+  float steps_per_mm_div_2_acc = block->step_event_count/(2*settings.acceleration*block->millimeters); 
-    (0.5*block->millimeters+(entry_speed*entry_speed-exit_speed*exit_speed)/(4*settings.acceleration)) );  
+  
+  // First determine intersection distance (in steps) from the exit point for a triangular profile.
+  // Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) )
+  int32_t intersect_distance = ceil( 0.5*(block->step_event_count + steps_per_mm_div_2_acc*(entry_speed*entry_speed-exit_speed*exit_speed)) );  
   
   // Check if this is a pure acceleration block by a intersection distance less than zero. Also
   // prevents signed and unsigned integer conversion errors.
   if (intersect_distance <= 0) {
     block->decelerate_after = 0; 
   } else {
-    // Determine deceleration distance from nominal speed to exit speed for a trapezoidal profile.
+    // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile.
     // Value is never negative. Nominal speed is always greater than or equal to the exit speed.
-    block->decelerate_after = ceil(steps_per_mm *
+    // Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) )
-      (block->nominal_speed*block->nominal_speed-exit_speed*exit_speed)/(2*settings.acceleration));
+    block->decelerate_after = ceil(steps_per_mm_div_2_acc * 
+                                   (block->nominal_speed*block->nominal_speed-exit_speed*exit_speed));
 
     // The lesser of the two triangle and trapezoid distances always defines the velocity profile.
     if (block->decelerate_after > intersect_distance) { block->decelerate_after = intersect_distance; }
@@ -168,58 +171,50 @@ static void planner_recalculate()
   // for feed-rate overrides, something like this is essential. Place a request here to the stepper driver to
   // find out where in the planner buffer is the a safe place to begin re-planning from.
 
-  // Perform reverse planner pass.
+  // Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the
+  // tail(first) block to prevent over-writing of the initial entry speed.
   uint8_t block_index = block_buffer_head;
-  block_t *next = NULL;
+  block_t *current = &block_buffer[block_index]; // Head block.
-  block_t *current = NULL;
+  block_t *next;
-  block_t *previous = NULL;
+  if (block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); }
-
   while (block_index != block_buffer_tail) {
-    block_index = prev_block_index( block_index );
     next = current;
-    current = previous;
+    current = &block_buffer[block_index];    
-    previous = &block_buffer[block_index];
-    
-    if (current) {  // Cannot operate on nothing.
-      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, max junction speed is guaranteed to be reached. Only compute
       // for max allowable speed if block is decelerating and nominal length is false.
       if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed)) {
         current->entry_speed = min( current->max_entry_speed,
-              max_allowable_speed(-settings.acceleration,next->entry_speed,current->millimeters));
+          calculate_final_velocity(settings.acceleration,next->entry_speed,current->millimeters)); // Back-compute
       } else {
         current->entry_speed = current->max_entry_speed;
       } 
       current->recalculate_flag = true;        
+    }
+    block_index = prev_block_index( block_index );
+  } 
   
-        }
+  // Perform forward planner pass. Begins junction speed adjustments after tail(first) block.
-      } // Skip last block. Already initialized and set for recalculation.
+  // Also recalculate trapezoids, block by block, as the forward pass completes the plan.
-    }
-  }
-  // Skip buffer tail/first block to prevent over-writing the initial entry speed.
-
-  // Perform forward planner pass.
   block_index = block_buffer_tail;
   next = NULL;
   while (block_index != block_buffer_head) {
     current = next;
     next= &block_buffer[block_index];
 
-    // Begin planning after buffer_tail
     if (current) {
-      // If the previous block is an acceleration block, but it is not long enough to complete the
+    
+      // If the current 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 (!current->nominal_length_flag) {
-        if (current->entry_speed < current->entry_speed) {
+        if (current->entry_speed < next->entry_speed) {
           float entry_speed = min( next->entry_speed,
-            max_allowable_speed(-settings.acceleration,current->entry_speed,current->millimeters) );
+            calculate_final_velocity(settings.acceleration,current->entry_speed,current->millimeters) );
     
           // Check for junction speed change
           if (next->entry_speed != entry_speed) {
@@ -227,18 +222,7 @@ static void planner_recalculate()
             next->recalculate_flag = true;
           }
         }
-      }      
-    }
-    block_index = next_block_index( block_index );
-  }
         
-  // Recalculate stepper algorithm data for any adjacent blocks with a modified junction speed.
-  block_index = block_buffer_tail;
-  next = NULL;  
-  while(block_index != block_buffer_head) {
-    current = next;
-    next = &block_buffer[block_index];
-    if (current) {
         // Recalculate if current block entry or exit junction speed has changed.
         if (current->recalculate_flag || next->recalculate_flag) {
           // NOTE: Entry and exit factors always > 0 by all previous logic operations.     
@@ -246,6 +230,8 @@ static void planner_recalculate()
           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 set with MINIMUM_PLANNER_SPEED. Always recalculated.
@@ -312,12 +298,6 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
   target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]);
   target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]);     
   
-  // Compute direction bits for this block
-  block->direction_bits = 0;
-  if (target[X_AXIS] < pl.position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
-  if (target[Y_AXIS] < pl.position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
-  if (target[Z_AXIS] < pl.position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
-  
   // Number of steps for each axis
   block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]);
   block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]);
@@ -328,31 +308,50 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
   if (block->step_event_count == 0) { return; };
   
   // Compute path vector in terms of absolute step target and current positions
+  // TODO: Store last xyz in memory to remove steps_per_mm divides. Or come up with another way to save cycles.
   float delta_mm[3];
-  delta_mm[X_AXIS] = (target[X_AXIS]-pl.position[X_AXIS])/settings.steps_per_mm[X_AXIS];
+  delta_mm[X_AXIS] = block->steps_x/settings.steps_per_mm[X_AXIS];
-  delta_mm[Y_AXIS] = (target[Y_AXIS]-pl.position[Y_AXIS])/settings.steps_per_mm[Y_AXIS];
+  delta_mm[Y_AXIS] = block->steps_y/settings.steps_per_mm[Y_AXIS];
-  delta_mm[Z_AXIS] = (target[Z_AXIS]-pl.position[Z_AXIS])/settings.steps_per_mm[Z_AXIS];
+  delta_mm[Z_AXIS] = block->steps_z/settings.steps_per_mm[Z_AXIS];
   block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + 
                             delta_mm[Z_AXIS]*delta_mm[Z_AXIS]);
   float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides	
 
-  // Calculate speed in mm/minute for each axis. No divide by zero due to previous checks.
-  // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
-  if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; }
-  block->nominal_speed = feed_rate; // (mm/min) Always > 0
-  
-  // Compute the acceleration, nominal rate, and distance traveled per step event for the stepper algorithm.
-  block->rate_delta = ceil(settings.acceleration*
-    ((RANADE_MULTIPLIER/(60*60))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
-  block->nominal_rate = ceil(block->nominal_speed*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
-  block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
-  
   // Compute path unit vector                            
   float unit_vec[3];
   unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
   unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
   unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;  
 
+  // Compute direction bits and correct unit vector directions
+  block->direction_bits = 0;
+  if (target[X_AXIS] < pl.position[X_AXIS]) { 
+    block->direction_bits |= (1<<X_DIRECTION_BIT); 
+    unit_vec[X_AXIS] = -unit_vec[X_AXIS];
+  }
+  if (target[Y_AXIS] < pl.position[Y_AXIS]) {
+    block->direction_bits |= (1<<Y_DIRECTION_BIT);
+    unit_vec[Y_AXIS] = -unit_vec[Y_AXIS];
+  }
+  if (target[Z_AXIS] < pl.position[Z_AXIS]) { 
+    block->direction_bits |= (1<<Z_DIRECTION_BIT);
+    unit_vec[Z_AXIS] = -unit_vec[Z_AXIS];
+  }
+  
+  // Calculate speed in mm/minute for each axis. No divide by zero due to previous checks.
+  // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
+  if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; }
+  block->nominal_speed = feed_rate; // (mm/min) Always > 0
+  
+  // TODO: When acceleration independence is installed, it can be kept in terms of 2*acceleration for 
+  // each block. This could save some 2*acceleration multiplications elsewhere. Need to check though.
+  
+  // Compute the acceleration, nominal rate, and distance traveled per step event for the stepper algorithm.
+  block->rate_delta = ceil(settings.acceleration*
+    ((RANADE_MULTIPLIER/(60*60))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
+  block->nominal_rate = ceil(block->nominal_speed*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
+  block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
+  
   // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
   // 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, 
@@ -392,8 +391,8 @@ void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert
   }
   block->max_entry_speed = vmax_junction;
   
-  // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
+  // Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED.
-  float v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
+  float v_allowable = calculate_final_velocity(settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
   block->entry_speed = min(vmax_junction, v_allowable);
 
   // Initialize planner efficiency flags

report.c

@@ -296,7 +296,7 @@ void report_realtime_status()
   // Report current machine state
   switch (sys.state) {
     case STATE_IDLE: printPgmString(PSTR("<Idle")); break;
-//    case STATE_INIT: printPgmString(PSTR("[Init")); break; // Never observed
+//    case STATE_INIT: printPgmString(PSTR("<Init")); break; // Never observed
     case STATE_QUEUED: printPgmString(PSTR("<Queue")); break;
     case STATE_CYCLE: printPgmString(PSTR("<Run")); break;
     case STATE_HOLD: printPgmString(PSTR("<Hold")); break;

stepper.c

@@ -106,8 +106,7 @@ void st_wake_up()
 // Stepper shutdown
 void st_go_idle() 
 {
-  // Disable stepper driver interrupt. Allow Timer2 to continue counting since COMPB may not be 
+  // Disable stepper driver interrupt. Allow Timer0 to finish. It will disable itself.
-  // finished. COMPB will disable itself when finished.
   TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt
   TCCR2B = 0; // Disable Timer2
 
@@ -131,7 +130,7 @@ void st_go_idle()
 // step event. However, the Ranade algorithm, as described, is susceptible to numerical round-off,
 // meaning that some axes steps may not execute for a given multi-axis motion.
 //   Grbl's algorithm slightly differs by using a single Ranade time-distance counter to manage
-// a Bresenham line algorithm for multi-axis step events and still ensure number of steps for
+// a Bresenham line algorithm for multi-axis step events which ensures the number of steps for
 // each axis are executed exactly. In other words, it uses a Bresenham within a Bresenham algorithm,
 // where one tracks time(Ranade) and the other steps.
 //   This interrupt pops blocks from the block_buffer and executes them by pulsing the stepper pins
@@ -146,7 +145,7 @@ ISR(TIMER2_COMPA_vect)
 // SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
   if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
   
-  // Set direction pins. Direction pins will always be set one timer tick before any step pulse.
+  // Set direction pins. New block dir will always be set one timer tick before any step pulse.
   STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
 
   // Pulse stepper pins, if flagged.
@@ -158,7 +157,7 @@ ISR(TIMER2_COMPA_vect)
   }
   
   busy = true;
-  sei();
+  sei(); // Re-enable interrupts. This ISR will still finish before returning to main program.
   
   // If there is no current block, attempt to pop one from the buffer
   if (current_block == NULL) {