Merge pull request #4387 from thinkyhead/rc_anhardt_more_4370

Update Z in a unified way in run_z_probe

Marlin/Marlin_main.cpp

 void get_available_commands();
 void process_next_command();
 void prepare_move_to_destination();
-void set_current_from_steppers();
+void set_current_from_steppers_for_axis(AxisEnum axis);
 
 #if ENABLED(ARC_SUPPORT)
   void plan_arc(float target[NUM_AXIS], float* offset, uint8_t clockwise);
     if (axis == X_AXIS || axis == Y_AXIS) {
 
       float homeposition[3];
-      for (uint8_t i = X_AXIS; i <= Z_AXIS; i++)
-        homeposition[i] = LOGICAL_POSITION(base_home_pos(i), i);
+      LOOP_XYZ(i) homeposition[i] = LOGICAL_POSITION(base_home_pos(i), i);
 
       // SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]);
       // SERIAL_ECHOPGM("homeposition[y]= "); SERIAL_ECHOLN(homeposition[1]);
     return false;
   }
 
-  #if ENABLED(DELTA)
-    #define SET_Z_FROM_STEPPERS() set_current_from_steppers()
-  #else
-    #define SET_Z_FROM_STEPPERS() current_position[Z_AXIS] = LOGICAL_POSITION(stepper.get_axis_position_mm(Z_AXIS), Z_AXIS)
-  #endif
-
   // Do a single Z probe and return with current_position[Z_AXIS]
   // at the height where the probe triggered.
   static float run_z_probe() {
       planner.bed_level_matrix.set_to_identity();
     #endif
 
-    #if ENABLED(DELTA)
-      float z_before = current_position[Z_AXIS],         // Current Z
-            z_mm = stepper.get_axis_position_mm(Z_AXIS); // Some tower's current position
-    #endif
-
     do_blocking_move_to_z(-(Z_MAX_LENGTH + 10), Z_PROBE_SPEED_FAST);
     endstops.hit_on_purpose();
-    SET_Z_FROM_STEPPERS();
+    set_current_from_steppers_for_axis(Z_AXIS);
     SYNC_PLAN_POSITION_KINEMATIC();
 
     // move up the retract distance
     do_blocking_move_to_z(current_position[Z_AXIS] + home_bump_mm(Z_AXIS), Z_PROBE_SPEED_FAST);
 
-    #if ENABLED(DELTA)
-      z_before = current_position[Z_AXIS];
-      z_mm = stepper.get_axis_position_mm(Z_AXIS);
-    #endif
-
     // move back down slowly to find bed
     do_blocking_move_to_z(current_position[Z_AXIS] - home_bump_mm(Z_AXIS) * 2, Z_PROBE_SPEED_SLOW);
     endstops.hit_on_purpose();
-    SET_Z_FROM_STEPPERS();
+    set_current_from_steppers_for_axis(Z_AXIS);
     SYNC_PLAN_POSITION_KINEMATIC();
 
     #if ENABLED(DEBUG_LEVELING_FEATURE)
  *  - Set the feedrate, if included
  */
 void gcode_get_destination() {
-  for (int i = 0; i < NUM_AXIS; i++) {
+  LOOP_XYZE(i) {
     if (code_seen(axis_codes[i]))
       destination[i] = code_value_axis_units(i) + (axis_relative_modes[i] || relative_mode ? current_position[i] : 0);
     else
   if (!didE) stepper.synchronize();
 
   bool didXYZ = false;
-  for (int i = 0; i < NUM_AXIS; i++) {
+  LOOP_XYZE(i) {
     if (code_seen(axis_codes[i])) {
       float p = current_position[i],
             v = code_value_axis_units(i);
  *      (Follows the same syntax as G92)
  */
 inline void gcode_M92() {
-  for (int8_t i = 0; i < NUM_AXIS; i++) {
+  LOOP_XYZE(i) {
     if (code_seen(axis_codes[i])) {
       if (i == E_AXIS) {
         float value = code_value_per_axis_unit(i);
  * M201: Set max acceleration in units/s^2 for print moves (M201 X1000 Y1000)
  */
 inline void gcode_M201() {
-  for (int8_t i = 0; i < NUM_AXIS; i++) {
+  LOOP_XYZE(i) {
     if (code_seen(axis_codes[i])) {
       planner.max_acceleration_mm_per_s2[i] = code_value_axis_units(i);
     }
 
 #if 0 // Not used for Sprinter/grbl gen6
   inline void gcode_M202() {
-    for (int8_t i = 0; i < NUM_AXIS; i++) {
+    LOOP_XYZE(i) {
       if (code_seen(axis_codes[i])) axis_travel_steps_per_sqr_second[i] = code_value_axis_units(i) * planner.axis_steps_per_mm[i];
     }
   }
  * M203: Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in units/sec
  */
 inline void gcode_M203() {
-  for (int8_t i = 0; i < NUM_AXIS; i++)
+  LOOP_XYZE(i)
     if (code_seen(axis_codes[i]))
       planner.max_feedrate_mm_s[i] = code_value_axis_units(i);
 }
  * M206: Set Additional Homing Offset (X Y Z). SCARA aliases T=X, P=Y
  */
 inline void gcode_M206() {
-  for (int8_t i = X_AXIS; i <= Z_AXIS; i++)
+  LOOP_XYZ(i)
     if (code_seen(axis_codes[i]))
       set_home_offset((AxisEnum)i, code_value_axis_units(i));
 
         SERIAL_ECHOLNPGM(">>> gcode_M666");
       }
     #endif
-    for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
+    LOOP_XYZ(i) {
       if (code_seen(axis_codes[i])) {
         endstop_adj[i] = code_value_axis_units(i);
         #if ENABLED(DEBUG_LEVELING_FEATURE)
    * M365: SCARA calibration: Scaling factor, X, Y, Z axis
    */
   inline void gcode_M365() {
-    for (int8_t i = X_AXIS; i <= Z_AXIS; i++)
+    LOOP_XYZ(i)
       if (code_seen(axis_codes[i]))
         axis_scaling[i] = code_value_float();
   }
   stepper.quick_stop();
   #if DISABLED(SCARA)
     stepper.synchronize();
-    set_current_from_steppers();
-    sync_plan_position();                       // ...re-apply to planner position
+    LOOP_XYZ(i) set_current_from_steppers_for_axis((AxisEnum)i);
+    SYNC_PLAN_POSITION_KINEMATIC();
   #endif
 }
 
  */
 inline void gcode_M428() {
   bool err = false;
-  for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
+  LOOP_XYZ(i) {
     if (axis_homed[i]) {
       float base = (current_position[i] > (sw_endstop_min[i] + sw_endstop_max[i]) / 2) ? base_home_pos(i) : 0,
             diff = current_position[i] - LOGICAL_POSITION(base, i);
     float lastpos[NUM_AXIS];
 
     // Save current position of all axes
-    for (uint8_t i = 0; i < NUM_AXIS; i++)
+    LOOP_XYZE(i)
       lastpos[i] = destination[i] = current_position[i];
 
     // Define runplan for move axes
  */
 inline void gcode_M907() {
   #if HAS_DIGIPOTSS
-    for (int i = 0; i < NUM_AXIS; i++)
+    LOOP_XYZE(i)
       if (code_seen(axis_codes[i])) stepper.digipot_current(i, code_value_int());
     if (code_seen('B')) stepper.digipot_current(4, code_value_int());
     if (code_seen('S')) for (int i = 0; i <= 4; i++) stepper.digipot_current(i, code_value_int());
   #endif
   #if ENABLED(DIGIPOT_I2C)
     // this one uses actual amps in floating point
-    for (int i = 0; i < NUM_AXIS; i++) if (code_seen(axis_codes[i])) digipot_i2c_set_current(i, code_value_float());
+    LOOP_XYZE(i) if (code_seen(axis_codes[i])) digipot_i2c_set_current(i, code_value_float());
     // for each additional extruder (named B,C,D,E..., channels 4,5,6,7...)
     for (int i = NUM_AXIS; i < DIGIPOT_I2C_NUM_CHANNELS; i++) if (code_seen('B' + i - (NUM_AXIS))) digipot_i2c_set_current(i, code_value_float());
   #endif
       float dac_percent = code_value_float();
       for (uint8_t i = 0; i <= 4; i++) dac_current_percent(i, dac_percent);
     }
-    for (uint8_t i = 0; i < NUM_AXIS; i++) if (code_seen(axis_codes[i])) dac_current_percent(i, code_value_float());
+    LOOP_XYZE(i) if (code_seen(axis_codes[i])) dac_current_percent(i, code_value_float());
   #endif
 }
 
   // M350 Set microstepping mode. Warning: Steps per unit remains unchanged. S code sets stepping mode for all drivers.
   inline void gcode_M350() {
     if (code_seen('S')) for (int i = 0; i <= 4; i++) stepper.microstep_mode(i, code_value_byte());
-    for (int i = 0; i < NUM_AXIS; i++) if (code_seen(axis_codes[i])) stepper.microstep_mode(i, code_value_byte());
+    LOOP_XYZE(i) if (code_seen(axis_codes[i])) stepper.microstep_mode(i, code_value_byte());
     if (code_seen('B')) stepper.microstep_mode(4, code_value_byte());
     stepper.microstep_readings();
   }
   inline void gcode_M351() {
     if (code_seen('S')) switch (code_value_byte()) {
       case 1:
-        for (int i = 0; i < NUM_AXIS; i++) if (code_seen(axis_codes[i])) stepper.microstep_ms(i, code_value_byte(), -1);
+        LOOP_XYZE(i) if (code_seen(axis_codes[i])) stepper.microstep_ms(i, code_value_byte(), -1);
         if (code_seen('B')) stepper.microstep_ms(4, code_value_byte(), -1);
         break;
       case 2:
-        for (int i = 0; i < NUM_AXIS; i++) if (code_seen(axis_codes[i])) stepper.microstep_ms(i, -1, code_value_byte());
+        LOOP_XYZE(i) if (code_seen(axis_codes[i])) stepper.microstep_ms(i, -1, code_value_byte());
         if (code_seen('B')) stepper.microstep_ms(4, -1, code_value_byte());
         break;
     }
 
 #endif // DELTA
 
-void set_current_from_steppers() {
+void set_current_from_steppers_for_axis(AxisEnum axis) {
   #if ENABLED(DELTA)
     set_cartesian_from_steppers();
-    current_position[X_AXIS] = cartesian_position[X_AXIS];
-    current_position[Y_AXIS] = cartesian_position[Y_AXIS];
-    current_position[Z_AXIS] = cartesian_position[Z_AXIS];
+    current_position[axis] = LOGICAL_POSITION(cartesian_position[axis], axis);
   #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
-    vector_3 pos = planner.adjusted_position(); // values directly from steppers...
-    current_position[X_AXIS] = pos.x;
-    current_position[Y_AXIS] = pos.y;
-    current_position[Z_AXIS] = pos.z;
+    vector_3 pos = planner.adjusted_position();
+    current_position[axis] = LOGICAL_POSITION(axis == X_AXIS ? pos.x : axis == Y_AXIS ? pos.y : pos.z, axis);
   #else
-    current_position[X_AXIS] = stepper.get_axis_position_mm(X_AXIS); // CORE handled transparently
-    current_position[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
-    current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
+    current_position[axis] = LOGICAL_POSITION(stepper.get_axis_position_mm(axis), axis); // CORE handled transparently
   #endif
-
-  for (uint8_t i = X_AXIS; i <= Z_AXIS; i++)
-    current_position[i] += LOGICAL_POSITION(0, i);
 }
 
 #if ENABLED(MESH_BED_LEVELING)
 
   inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) {
     float difference[NUM_AXIS];
-    for (int8_t i = 0; i < NUM_AXIS; i++) difference[i] = target[i] - current_position[i];
+    LOOP_XYZE(i) difference[i] = target[i] - current_position[i];
 
     float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
     if (cartesian_mm < 0.000001) cartesian_mm = abs(difference[E_AXIS]);
 
       float fraction = float(s) * inv_steps;
 
-      for (int8_t i = 0; i < NUM_AXIS; i++)
+      LOOP_XYZE(i)
         target[i] = current_position[i] + difference[i] * fraction;
 
       inverse_kinematics(target);

Marlin/configuration_store.cpp

   float tmp1[] = DEFAULT_AXIS_STEPS_PER_UNIT;
   float tmp2[] = DEFAULT_MAX_FEEDRATE;
   long tmp3[] = DEFAULT_MAX_ACCELERATION;
-  for (uint8_t i = 0; i < NUM_AXIS; i++) {
+  LOOP_XYZE(i) {
     planner.axis_steps_per_mm[i] = tmp1[i];
     planner.max_feedrate_mm_s[i] = tmp2[i];
     planner.max_acceleration_mm_per_s2[i] = tmp3[i];

Marlin/enum.h

   Z_HEAD  = 5
 };
 
+#define LOOP_XYZ(VAR)  for (uint8_t VAR=X_AXIS; VAR<=Z_AXIS; VAR++)
+#define LOOP_XYZE(VAR) for (uint8_t VAR=X_AXIS; VAR<=E_AXIS; VAR++)
+
 typedef enum {
   LINEARUNIT_MM,
   LINEARUNIT_INCH

Marlin/planner.cpp

 void Planner::init() {
   block_buffer_head = block_buffer_tail = 0;
   memset(position, 0, sizeof(position)); // clear position
-  for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = 0.0;
+  LOOP_XYZE(i) previous_speed[i] = 0.0;
   previous_nominal_speed = 0.0;
   #if ENABLED(AUTO_BED_LEVELING_FEATURE)
     bed_level_matrix.set_to_identity();
 
     for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
       block = &block_buffer[b];
-      for (int i = 0; i < NUM_AXIS; i++) if (block->steps[i]) axis_active[i]++;
+      LOOP_XYZE(i) if (block->steps[i]) axis_active[i]++;
     }
   }
   #if ENABLED(DISABLE_X)
   // Calculate and limit speed in mm/sec for each axis
   float current_speed[NUM_AXIS];
   float speed_factor = 1.0; //factor <=1 do decrease speed
-  for (int i = 0; i < NUM_AXIS; i++) {
+  LOOP_XYZE(i) {
     current_speed[i] = delta_mm[i] * inverse_second;
     float cs = fabs(current_speed[i]), mf = max_feedrate_mm_s[i];
     if (cs > mf) speed_factor = min(speed_factor, mf / cs);
 
   // Correct the speed
   if (speed_factor < 1.0) {
-    for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
+    LOOP_XYZE(i) current_speed[i] *= speed_factor;
     block->nominal_speed *= speed_factor;
     block->nominal_rate *= speed_factor;
   }
   block->recalculate_flag = true; // Always calculate trapezoid for new block
 
   // Update previous path unit_vector and nominal speed
-  for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
+  LOOP_XYZE(i) previous_speed[i] = current_speed[i];
   previous_nominal_speed = block->nominal_speed;
 
   #if ENABLED(LIN_ADVANCE)
   block_buffer_head = next_buffer_head;
 
   // Update position
-  for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];
+  LOOP_XYZE(i) position[i] = target[i];
 
   recalculate();
 
     stepper.set_position(nx, ny, nz, ne);
     previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
 
-    for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = 0.0;
+    LOOP_XYZE(i) previous_speed[i] = 0.0;
   }
 
 /**
 
 // Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
 void Planner::reset_acceleration_rates() {
-  for (int i = 0; i < NUM_AXIS; i++)
+  LOOP_XYZE(i)
     max_acceleration_steps_per_s2[i] = max_acceleration_mm_per_s2[i] * axis_steps_per_mm[i];
 }