Cleanup of planner code

- Use named axis indexes, `X_AXIS` etc.
- Replace `block.steps_A` with block.steps[A]`
- Replace `A_segment_time` with `segment_time[A]`
- Add `A_AXIS`, `B_AXIS` for `COREXY` axes
- Conditional compile based on `EXTRUDERS`
- Add BLOCK_MOD macro for planner block indexes
- Apply coding standards to `planner.h` and `planner.cpp`
- Small optimizations of planner code
- Update `stepper.cpp` for new `block` struct
- Replace `memcpy` with loops, let the compiler unroll them
- Make `movesplanned` into an inline function

Marlin/Marlin.h

@@ -183,7 +183,7 @@ void manage_inactivity(bool ignore_stepper_queue=false);
   #define disable_e3() /* nothing */
 #endif
 
-enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5}; 
+enum AxisEnum {X_AXIS=0, Y_AXIS=1, A_AXIS=0, B_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5};
 //X_HEAD and Y_HEAD is used for systems that don't have a 1:1 relationship between X_AXIS and X Head movement, like CoreXY bots.
 
 void FlushSerialRequestResend();
@@ -270,7 +270,7 @@ extern unsigned char fanSpeedSoftPwm;
   extern bool filament_sensor;  //indicates that filament sensor readings should control extrusion
   extern float filament_width_meas; //holds the filament diameter as accurately measured
   extern signed char measurement_delay[];  //ring buffer to delay measurement
-  extern int delay_index1, delay_index2;  //index into ring buffer
+  extern int delay_index1, delay_index2;  //ring buffer index. used by planner, temperature, and main code
   extern float delay_dist; //delay distance counter
   extern int meas_delay_cm; //delay distance
 #endif

Marlin/planner.h

@@ -21,20 +21,16 @@
 // This module is to be considered a sub-module of stepper.c. Please don't include 
 // this file from any other module.
 
-#ifndef planner_h
-#define planner_h
+#ifndef PLANNER_H
+#define PLANNER_H
 
 #include "Marlin.h"
 
-#ifdef ENABLE_AUTO_BED_LEVELING
-#include "vector_3.h"
-#endif // ENABLE_AUTO_BED_LEVELING
-
 // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in 
 // the source g-code and may never actually be reached if acceleration management is active.
 typedef struct {
   // Fields used by the bresenham algorithm for tracing the line
-  long steps_x, steps_y, steps_z, steps_e;  // Step count along each axis
+  long steps[NUM_AXIS];                     // Step count along each axis
   unsigned long step_event_count;           // The number of step events required to complete this block
   long accelerate_until;                    // The index of the step event on which to stop acceleration
   long decelerate_after;                    // The index of the step event on which to start decelerating
@@ -49,7 +45,7 @@ typedef struct {
   #endif
 
   // Fields used by the motion planner to manage acceleration
-//  float speed_x, speed_y, speed_z, speed_e;        // Nominal mm/sec for each axis
+  // float speed_x, speed_y, speed_z, speed_e;          // Nominal mm/sec for each axis
   float nominal_speed;                               // The nominal speed for this block in mm/sec 
   float entry_speed;                                 // Entry speed at previous-current junction in mm/sec
   float max_entry_speed;                             // Maximum allowable junction entry speed in mm/sec
@@ -65,48 +61,44 @@ typedef struct {
   unsigned long acceleration_st;                     // acceleration steps/sec^2
   unsigned long fan_speed;
   #ifdef BARICUDA
-  unsigned long valve_pressure;
-  unsigned long e_to_p_pressure;
+    unsigned long valve_pressure;
+    unsigned long e_to_p_pressure;
   #endif
   volatile char busy;
 } block_t;
 
-#ifdef ENABLE_AUTO_BED_LEVELING
-// this holds the required transform to compensate for bed level
-extern matrix_3x3 plan_bed_level_matrix;
-#endif // #ifdef ENABLE_AUTO_BED_LEVELING
+#define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
 
 // Initialize the motion plan subsystem      
 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.
+void check_axes_activity();
 
-#ifdef ENABLE_AUTO_BED_LEVELING
-void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
+// Get the number of buffered moves
+extern volatile unsigned char block_buffer_head;
+extern volatile unsigned char block_buffer_tail;
+FORCE_INLINE uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
 
+#ifdef ENABLE_AUTO_BED_LEVELING
+  #include "vector_3.h"
+  // this holds the required transform to compensate for bed level
+  extern matrix_3x3 plan_bed_level_matrix;
+  // 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.
+  void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
+  // Set position. Used for G92 instructions.
+  void plan_set_position(float x, float y, float z, const float &e);
   #ifndef DELTA
-  // Get the position applying the bed level matrix if enabled
-  vector_3 plan_get_position();
+    // Get the position applying the bed level matrix if enabled
+    vector_3 plan_get_position();
   #endif
-#else
-void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
-#endif // ENABLE_AUTO_BED_LEVELING
-
-// Set position. Used for G92 instructions.
-#ifdef ENABLE_AUTO_BED_LEVELING
-void plan_set_position(float x, float y, float z, const float &e);
-#else
-void plan_set_position(const float &x, const float &y, const float &z, const float &e);
-#endif // ENABLE_AUTO_BED_LEVELING
+#else //!ENABLE_AUTO_BED_LEVELING
+  void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
+  void plan_set_position(const float &x, const float &y, const float &z, const float &e);
+#endif //!ENABLE_AUTO_BED_LEVELING
 
 void plan_set_e_position(const float &e);
 
-
-
-void check_axes_activity();
-uint8_t movesplanned(); //return the nr of buffered moves
-
 extern unsigned long minsegmenttime;
 extern float max_feedrate[NUM_AXIS]; // set the max speeds
 extern float axis_steps_per_unit[NUM_AXIS];
@@ -122,44 +114,41 @@ extern float mintravelfeedrate;
 extern unsigned long axis_steps_per_sqr_second[NUM_AXIS];
 
 #ifdef AUTOTEMP
-    extern bool autotemp_enabled;
-    extern float autotemp_max;
-    extern float autotemp_min;
-    extern float autotemp_factor;
+  extern bool autotemp_enabled;
+  extern float autotemp_max;
+  extern float autotemp_min;
+  extern float autotemp_factor;
 #endif
 
-    
-
-
-extern block_t block_buffer[BLOCK_BUFFER_SIZE];            // A ring buffer for motion instfructions
+extern block_t block_buffer[BLOCK_BUFFER_SIZE];            // A ring buffer for motion instructions
 extern volatile unsigned char block_buffer_head;           // Index of the next block to be pushed
 extern volatile unsigned char block_buffer_tail; 
-// Called when the current block is no longer needed. Discards the block and makes the memory
-// availible for new blocks.    
-FORCE_INLINE void plan_discard_current_block()  
-{
-  if (block_buffer_head != block_buffer_tail) {
-    block_buffer_tail = (block_buffer_tail + 1) & (BLOCK_BUFFER_SIZE - 1);  
-  }
+
+// Returns true if the buffer has a queued block, false otherwise
+FORCE_INLINE bool blocks_queued() { return (block_buffer_head != block_buffer_tail); }
+
+// Called when the current block is no longer needed. Discards
+// the block and makes the memory available for new blocks.
+FORCE_INLINE void plan_discard_current_block() {
+  if (blocks_queued())
+    block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
 }
 
 // Gets the current block. Returns NULL if buffer empty
-FORCE_INLINE block_t *plan_get_current_block() 
-{
-  if (block_buffer_head == block_buffer_tail) { 
-    return(NULL); 
+FORCE_INLINE block_t *plan_get_current_block() {
+  if (blocks_queued()) {
+    block_t *block = &block_buffer[block_buffer_tail];
+    block->busy = true;
+    return block;
   }
-  block_t *block = &block_buffer[block_buffer_tail];
-  block->busy = true;
-  return(block);
+  else
+    return NULL;
 }
 
-// Returns true if the buffer has a queued block, false otherwise
-FORCE_INLINE bool blocks_queued() { return (block_buffer_head != block_buffer_tail); }
-
 #ifdef PREVENT_DANGEROUS_EXTRUDE
-void set_extrude_min_temp(float temp);
+  void set_extrude_min_temp(float temp);
 #endif
 
 void reset_acceleration_rates();
-#endif
+
+#endif //PLANNER_H

Marlin/stepper.cpp

@@ -370,7 +370,7 @@ ISR(TIMER1_COMPA_vect) {
       step_events_completed = 0;
 
       #ifdef Z_LATE_ENABLE
-        if (current_block->steps_z > 0) {
+        if (current_block->steps[Z_AXIS] > 0) {
           enable_z();
           OCR1A = 2000; //1ms wait
           return;
@@ -411,7 +411,7 @@ ISR(TIMER1_COMPA_vect) {
 
     #define UPDATE_ENDSTOP(axis,AXIS,minmax,MINMAX) \
       bool axis ##_## minmax ##_endstop = (READ(AXIS ##_## MINMAX ##_PIN) != AXIS ##_## MINMAX ##_ENDSTOP_INVERTING); \
-      if (axis ##_## minmax ##_endstop && old_## axis ##_## minmax ##_endstop && (current_block->steps_## axis > 0)) { \
+      if (axis ##_## minmax ##_endstop && old_## axis ##_## minmax ##_endstop && (current_block->steps[AXIS ##_AXIS] > 0)) { \
         endstops_trigsteps[AXIS ##_AXIS] = count_position[AXIS ##_AXIS]; \
         endstop_## axis ##_hit = true; \
         step_events_completed = current_block->step_event_count; \
@@ -420,54 +420,54 @@ ISR(TIMER1_COMPA_vect) {
 
     // Check X and Y endstops
     if (check_endstops) {
-      #ifndef COREXY
-        if (TEST(out_bits, X_AXIS))   // stepping along -X axis (regular cartesians bot)
-      #else
+      #ifdef COREXY
         // Head direction in -X axis for CoreXY bots.
         // If DeltaX == -DeltaY, the movement is only in Y axis
-        if (current_block->steps_x != current_block->steps_y || (TEST(out_bits, X_AXIS) == TEST(out_bits, Y_AXIS)))      
-            if (TEST(out_bits, X_HEAD))
-      #endif
-            { // -direction
-              #ifdef DUAL_X_CARRIAGE
-                // with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
-                if ((current_block->active_extruder == 0 && X_HOME_DIR == -1) || (current_block->active_extruder != 0 && X2_HOME_DIR == -1))
-              #endif          
-                {
-                  #if defined(X_MIN_PIN) && X_MIN_PIN >= 0
-                    UPDATE_ENDSTOP(x, X, min, MIN);
-                  #endif
-                }
-            }
-            else { // +direction
-              #ifdef DUAL_X_CARRIAGE
-                // with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
-                if ((current_block->active_extruder == 0 && X_HOME_DIR == 1) || (current_block->active_extruder != 0 && X2_HOME_DIR == 1))
-              #endif
-                {
-                  #if defined(X_MAX_PIN) && X_MAX_PIN >= 0
-                    UPDATE_ENDSTOP(x, X, max, MAX);
-                  #endif
-                }
-            }
-      #ifndef COREXY
-        if (TEST(out_bits, Y_AXIS))   // -direction
+        if (current_block->steps[A_AXIS] != current_block->steps[B_AXIS] || (TEST(out_bits, A_AXIS) == TEST(out_bits, B_AXIS)))
+          if (TEST(out_bits, X_HEAD))
       #else
+          if (TEST(out_bits, X_AXIS))   // stepping along -X axis (regular cartesians bot)
+      #endif
+          { // -direction
+            #ifdef DUAL_X_CARRIAGE
+              // with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
+              if ((current_block->active_extruder == 0 && X_HOME_DIR == -1) || (current_block->active_extruder != 0 && X2_HOME_DIR == -1))
+            #endif          
+              {
+                #if defined(X_MIN_PIN) && X_MIN_PIN >= 0
+                  UPDATE_ENDSTOP(x, X, min, MIN);
+                #endif
+              }
+          }
+          else { // +direction
+            #ifdef DUAL_X_CARRIAGE
+              // with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
+              if ((current_block->active_extruder == 0 && X_HOME_DIR == 1) || (current_block->active_extruder != 0 && X2_HOME_DIR == 1))
+            #endif
+              {
+                #if defined(X_MAX_PIN) && X_MAX_PIN >= 0
+                  UPDATE_ENDSTOP(x, X, max, MAX);
+                #endif
+              }
+          }
+      #ifdef COREXY
         // Head direction in -Y axis for CoreXY bots.
         // If DeltaX == DeltaY, the movement is only in X axis
-        if (current_block->steps_x != current_block->steps_y || (TEST(out_bits, X_AXIS) != TEST(out_bits, Y_AXIS)))
-            if (TEST(out_bits, Y_HEAD))             
+        if (current_block->steps[A_AXIS] != current_block->steps[B_AXIS] || (TEST(out_bits, A_AXIS) != TEST(out_bits, B_AXIS)))
+          if (TEST(out_bits, Y_HEAD))
+      #else
+          if (TEST(out_bits, Y_AXIS))   // -direction
       #endif
-            { // -direction
-              #if defined(Y_MIN_PIN) && Y_MIN_PIN >= 0
-                UPDATE_ENDSTOP(y, Y, min, MIN);
-              #endif
-            }
-            else { // +direction
-              #if defined(Y_MAX_PIN) && Y_MAX_PIN >= 0
-                UPDATE_ENDSTOP(y, Y, max, MAX);
-              #endif
-            }
+          { // -direction
+            #if defined(Y_MIN_PIN) && Y_MIN_PIN >= 0
+              UPDATE_ENDSTOP(y, Y, min, MIN);
+            #endif
+          }
+          else { // +direction
+            #if defined(Y_MAX_PIN) && Y_MAX_PIN >= 0
+              UPDATE_ENDSTOP(y, Y, max, MAX);
+            #endif
+          }
     }
 
     if (TEST(out_bits, Z_AXIS)) {   // -direction
@@ -515,7 +515,7 @@ ISR(TIMER1_COMPA_vect) {
       #endif
 
       #ifdef ADVANCE
-        counter_e += current_block->steps_e;
+        counter_e += current_block->steps[E_AXIS];
         if (counter_e > 0) {
           counter_e -= current_block->step_event_count;
           e_steps[current_block->active_extruder] += TEST(out_bits, E_AXIS) ? -1 : 1;
@@ -529,15 +529,14 @@ ISR(TIMER1_COMPA_vect) {
          * instead of doing each in turn. The extra tests add enough
          * lag to allow it work with without needing NOPs
          */
-        counter_x += current_block->steps_x;
-        if (counter_x > 0) X_STEP_WRITE(HIGH);
-        counter_y += current_block->steps_y;
-        if (counter_y > 0) Y_STEP_WRITE(HIGH);
-        counter_z += current_block->steps_z;
-        if (counter_z > 0) Z_STEP_WRITE(HIGH);
+        #define STEP_ADD(axis, AXIS) \
+         counter_## axis += current_block->steps[AXIS ##_AXIS]; \
+         if (counter_## axis > 0) { AXIS ##_STEP_WRITE(HIGH); }
+        STEP_ADD(x,X);
+        STEP_ADD(y,Y);
+        STEP_ADD(z,Z);
         #ifndef ADVANCE
-          counter_e += current_block->steps_e;
-          if (counter_e > 0) E_STEP_WRITE(HIGH);
+          STEP_ADD(e,E);
         #endif
 
         #define STEP_IF_COUNTER(axis, AXIS) \
@@ -557,7 +556,7 @@ ISR(TIMER1_COMPA_vect) {
       #else // !CONFIG_STEPPERS_TOSHIBA
 
         #define APPLY_MOVEMENT(axis, AXIS) \
-          counter_## axis += current_block->steps_## axis; \
+          counter_## axis += current_block->steps[AXIS ##_AXIS]; \
           if (counter_## axis > 0) { \
             AXIS ##_APPLY_STEP(!INVERT_## AXIS ##_STEP_PIN,0); \
             counter_## axis -= current_block->step_event_count; \