Fix Babystepping loop (again)

Marlin/src/feature/babystep.cpp

 
 Babystep babystep;
 
-volatile int16_t Babystep::steps[BS_TODO_AXIS(Z_AXIS) + 1];
+volatile int16_t Babystep::steps[BS_AXIS_IND(Z_AXIS) + 1];
 #if ENABLED(BABYSTEP_DISPLAY_TOTAL)
   int16_t Babystep::axis_total[BS_TOTAL_AXIS(Z_AXIS) + 1];
 #endif
 int16_t Babystep::accum;
 
 void Babystep::step_axis(const AxisEnum axis) {
-  const int16_t curTodo = steps[BS_TODO_AXIS(axis)]; // get rid of volatile for performance
+  const int16_t curTodo = steps[BS_AXIS_IND(axis)]; // get rid of volatile for performance
   if (curTodo) {
     stepper.do_babystep((AxisEnum)axis, curTodo > 0);
-    if (curTodo > 0) steps[BS_TODO_AXIS(axis)]--; else steps[BS_TODO_AXIS(axis)]++;
+    if (curTodo > 0) steps[BS_AXIS_IND(axis)]--; else steps[BS_AXIS_IND(axis)]++;
   }
 }
 
     #else
       BSA_ENABLE(Z_AXIS);
     #endif
-    steps[BS_TODO_AXIS(axis)] += distance;
+    steps[BS_AXIS_IND(axis)] += distance;
   #endif
   #if ENABLED(BABYSTEP_ALWAYS_AVAILABLE)
     gcode.reset_stepper_timeout();

Marlin/src/feature/babystep.h

 #endif
 
 #if IS_CORE || EITHER(BABYSTEP_XY, I2C_POSITION_ENCODERS)
-  #define BS_TODO_AXIS(A) A
+  #define BS_AXIS_IND(A) A
+  #define BS_AXIS(I) AxisEnum(I)
 #else
-  #define BS_TODO_AXIS(A) 0
+  #define BS_AXIS_IND(A) 0
+  #define BS_AXIS(I) Z_AXIS
 #endif
 
 #if ENABLED(BABYSTEP_DISPLAY_TOTAL)
 
 class Babystep {
 public:
-  static volatile int16_t steps[BS_TODO_AXIS(Z_AXIS) + 1];
+  static volatile int16_t steps[BS_AXIS_IND(Z_AXIS) + 1];
   static int16_t accum;                                     // Total babysteps in current edit
 
   #if ENABLED(BABYSTEP_DISPLAY_TOTAL)
   static void add_mm(const AxisEnum axis, const float &mm);
 
   static inline bool has_steps() {
-    return steps[BS_TODO_AXIS(X_AXIS)] || steps[BS_TODO_AXIS(Y_AXIS)] || steps[BS_TODO_AXIS(Z_AXIS)];
+    return steps[BS_AXIS_IND(X_AXIS)] || steps[BS_AXIS_IND(Y_AXIS)] || steps[BS_AXIS_IND(Z_AXIS)];
   }
 
   //
   // apply accumulated babysteps to the axes.
   //
   static inline void task() {
-    LOOP_LE_N(axis, BS_TODO_AXIS(Z_AXIS)) step_axis((AxisEnum)axis);
+    LOOP_LE_N(i, BS_AXIS_IND(Z_AXIS)) step_axis(BS_AXIS(i));
   }
 
 private:

Marlin/src/feature/tmc_util.cpp

 
   bool tmc_enable_stallguard(TMC2209Stepper &st) {
     const bool stealthchop_was_enabled = !st.en_spreadCycle();
-    
+
     st.TCOOLTHRS(0xFFFFF);
     st.en_spreadCycle(false);
     return stealthchop_was_enabled;

Marlin/src/gcode/motion/M290.cpp

         #else
           PSTR("Babystep Z")
         #endif
-        , babystep.axis_total[BS_TODO_AXIS(Z_AXIS)]
+        , babystep.axis_total[BS_AXIS_IND(Z_AXIS)]
       );
     }
     #endif

Marlin/src/lcd/extensible_ui/ui_api.cpp

     #if HAS_BED_PROBE
       return probe.offset.z;
     #elif ENABLED(BABYSTEP_DISPLAY_TOTAL)
-      return (planner.steps_to_mm[Z_AXIS] * babystep.axis_total[BS_TODO_AXIS(Z_AXIS)]);
+      return (planner.steps_to_mm[Z_AXIS] * babystep.axis_total[BS_AXIS_IND(Z_AXIS)]);
     #else
       return 0.0;
     #endif
     float getPIDValues_Kp(const extruder_t tool) {
       return PID_PARAM(Kp, tool);
     }
-    
+
     float getPIDValues_Ki(const extruder_t tool) {
       return unscalePID_i(PID_PARAM(Ki, tool));
     }
-    
+
     float getPIDValues_Kd(const extruder_t tool) {
       return unscalePID_d(PID_PARAM(Kd, tool));
     }
       thermalManager.PID_autotune(temp, (heater_ind_t)tool, 8, true);
     }
   #endif
-  
+
   #if ENABLED(PIDTEMPBED)
     float getBedPIDValues_Kp() {
       return thermalManager.temp_bed.pid.Kp;
     }
-    
+
     float getBedPIDValues_Ki() {
       return unscalePID_i(thermalManager.temp_bed.pid.Ki);
     }
-    
+
     float getBedPIDValues_Kd() {
       return unscalePID_d(thermalManager.temp_bed.pid.Kd);
     }
-    
+
     void setBedPIDValues(const float p, const float i, const float d) {
       thermalManager.temp_bed.pid.Kp = p;
       thermalManager.temp_bed.pid.Ki = scalePID_i(i);
       thermalManager.temp_bed.pid.Kd = scalePID_d(d);
       thermalManager.updatePID();
     }
-    
+
     void startBedPIDTune(const float temp) {
       thermalManager.PID_autotune(temp, H_BED, 4, true);
     }

Marlin/src/lcd/extensible_ui/ui_api.h

     void setPIDValues(const float, const float, const float, extruder_t);
     void startPIDTune(const float, extruder_t);
   #endif
-  
+
   #if ENABLED(PIDTEMPBED)
     float getBedPIDValues_Kp();
     float getBedPIDValues_Ki();

Marlin/src/module/stepper.cpp

 
 Stepper stepper; // Singleton
 
+#define BABYSTEPPING_EXTRA_DIR_WAIT
+
 #if HAS_MOTOR_CURRENT_PWM
   bool Stepper::initialized; // = false
 #endif
     #endif
 
     #if ENABLED(INTEGRATED_BABYSTEPPING)
-      const bool do_babystep = (nextBabystepISR == 0);              // 0 = Do Babystepping (XY)Z pulses
-      if (do_babystep) nextBabystepISR = babystepping_isr();
+      const bool is_babystep = (nextBabystepISR == 0);              // 0 = Do Babystepping (XY)Z pulses
+      if (is_babystep) nextBabystepISR = babystepping_isr();
     #endif
 
     // ^== Time critical. NOTHING besides pulse generation should be above here!!!
     if (!nextMainISR) nextMainISR = block_phase_isr();  // Manage acc/deceleration, get next block
 
     #if ENABLED(INTEGRATED_BABYSTEPPING)
-      if (do_babystep)                                  // Avoid ANY stepping too soon after baby-stepping
+      if (is_babystep)                                  // Avoid ANY stepping too soon after baby-stepping
         NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8);      // FULL STOP for 125µs after a baby-step
 
       if (nextBabystepISR != BABYSTEP_NEVER)            // Avoid baby-stepping too close to axis Stepping
     #endif
   #endif
 
+  #if ENABLED(BABYSTEPPING_EXTRA_DIR_WAIT)
+    #define EXTRA_DIR_WAIT_BEFORE DIR_WAIT_BEFORE
+    #define EXTRA_DIR_WAIT_AFTER  DIR_WAIT_AFTER
+  #else
+    #define EXTRA_DIR_WAIT_BEFORE()
+    #define EXTRA_DIR_WAIT_AFTER()
+  #endif
+
   #if DISABLED(DELTA)
 
     #define BABYSTEP_AXIS(AXIS, INV, DIR) do{           \
       _APPLY_STEP(AXIS, !_INVERT_STEP_PIN(AXIS), true); \
       _PULSE_WAIT();                                    \
       _APPLY_STEP(AXIS, _INVERT_STEP_PIN(AXIS), true);  \
-      DIR_WAIT_BEFORE();                                \
+      EXTRA_DIR_WAIT_BEFORE();                          \
       _APPLY_DIR(AXIS, old_dir);                        \
-      DIR_WAIT_AFTER();                                 \
+      EXTRA_DIR_WAIT_AFTER();                           \
     }while(0)
 
   #elif IS_CORE
       _PULSE_WAIT();                                            \
       _APPLY_STEP(A, _INVERT_STEP_PIN(A), true);                \
       _APPLY_STEP(B, _INVERT_STEP_PIN(B), true);                \
-      DIR_WAIT_BEFORE();                                        \
+      EXTRA_DIR_WAIT_BEFORE();                                  \
       _APPLY_DIR(A, old_dir.a); _APPLY_DIR(B, old_dir.b);       \
-      DIR_WAIT_AFTER();                                         \
+      EXTRA_DIR_WAIT_AFTER();                                   \
     }while(0)
 
   #endif
           Z_STEP_WRITE(INVERT_Z_STEP_PIN);
 
           // Restore direction bits
-          DIR_WAIT_BEFORE();
+          EXTRA_DIR_WAIT_BEFORE();
 
           X_DIR_WRITE(old_dir.x);
           Y_DIR_WRITE(old_dir.y);
           Z_DIR_WRITE(old_dir.z);
 
-          DIR_WAIT_AFTER();
+          EXTRA_DIR_WAIT_AFTER();
 
         #endif
 

Marlin/src/pins/mega/pins_PICA.h

 /*
 // Note that these are the "pins" that correspond to the analog inputs on the arduino mega.
 // These are not the same as the physical pin numbers
-  AD0 = 54;   AD1 = 55;   AD2 = 56;   AD3 = 57;   
-  AD4 = 58;   AD5 = 59;   AD6 = 60;   AD7 = 61;   
+  AD0 = 54;   AD1 = 55;   AD2 = 56;   AD3 = 57;
+  AD4 = 58;   AD5 = 59;   AD6 = 60;   AD7 = 61;
   AD8 = 62;   AD9 = 63;   AD10 = 64;  AD11 = 65;
   AD12 = 66;  AD13 = 67;  AD14 = 68;  AD15 = 69;
 */
 //
 #define X_STEP_PIN         55
 #define X_DIR_PIN          54
-#define X_ENABLE_PIN       60     
+#define X_ENABLE_PIN       60
 
 #define Y_STEP_PIN         57
 #define Y_DIR_PIN          56
 #define Z_DIR_PIN          58
 #define Z_ENABLE_PIN       62
 
-#define E0_STEP_PIN        67 
+#define E0_STEP_PIN        67
 #define E0_DIR_PIN         24
 #define E0_ENABLE_PIN      26
 
 #endif
 
 #define SDPOWER_PIN        -1
-#define LED_PIN            -1 
+#define LED_PIN            -1
 #define PS_ON_PIN          -1
 #define KILL_PIN           -1
-      
+
 #define SSR_PIN             6
 
 // SPI for Max6675 or Max31855 Thermocouple
 //
 // SD Support
 //
-#define SD_DETECT_PIN      49 
+#define SD_DETECT_PIN      49
 #define SDSS               53
 
 //