From fa00e1d97f2d82c719560c91e5001695e691ec12 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Mon, 15 Jun 2015 17:34:04 -0700
Subject: [PATCH] Explode conditions common to corexy and cartesian

---
 Marlin/planner.cpp | 24 ++++++++++++++----------
 1 file changed, 14 insertions(+), 10 deletions(-)

diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index d9a52c0bc..04f7fe575 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -541,13 +541,14 @@ float junction_deviation = 0.1;
     // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
     block->steps[A_AXIS] = labs(dx + dy);
     block->steps[B_AXIS] = labs(dx - dy);
+    block->steps[Z_AXIS] = labs(dz);
   #else
     // default non-h-bot planning
     block->steps[X_AXIS] = labs(dx);
     block->steps[Y_AXIS] = labs(dy);
+    block->steps[Z_AXIS] = labs(dz);
   #endif
 
-  block->steps[Z_AXIS] = labs(dz);
   block->steps[E_AXIS] = labs(de);
   block->steps[E_AXIS] *= volumetric_multiplier[extruder];
   block->steps[E_AXIS] *= extruder_multiplier[extruder];
@@ -568,13 +569,14 @@ float junction_deviation = 0.1;
   #ifdef COREXY
     if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
     if (dy < 0) db |= BIT(Y_HEAD); // ...and Y
+    if (dz < 0) db |= BIT(Z_AXIS);
     if (dx + dy < 0) db |= BIT(A_AXIS); // Motor A direction
     if (dx - dy < 0) db |= BIT(B_AXIS); // Motor B direction
   #else
     if (dx < 0) db |= BIT(X_AXIS);
     if (dy < 0) db |= BIT(Y_AXIS); 
+    if (dz < 0) db |= BIT(Z_AXIS);
   #endif
-  if (dz < 0) db |= BIT(Z_AXIS);
   if (de < 0) db |= BIT(E_AXIS); 
   block->direction_bits = db;
 
@@ -586,13 +588,15 @@ float junction_deviation = 0.1;
       enable_x();
       enable_y();
     }
+    #ifndef Z_LATE_ENABLE
+      if (block->steps[Z_AXIS]) enable_z();
+    #endif
   #else
     if (block->steps[X_AXIS]) enable_x();
     if (block->steps[Y_AXIS]) enable_y();
-  #endif
-
-  #ifndef Z_LATE_ENABLE
-    if (block->steps[Z_AXIS]) enable_z();
+    #ifndef Z_LATE_ENABLE
+      if (block->steps[Z_AXIS]) enable_z();
+    #endif
   #endif
 
   // Enable extruder(s)
@@ -676,14 +680,15 @@ float junction_deviation = 0.1;
     float delta_mm[6];
     delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
     delta_mm[Y_HEAD] = dy / axis_steps_per_unit[B_AXIS];
+    delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
     delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS];
     delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS];
   #else
     float delta_mm[4];
     delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS];
     delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
+    delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
   #endif
-  delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
   delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[extruder] * extruder_multiplier[extruder] / 100.0;
 
   if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) {
@@ -692,11 +697,10 @@ float junction_deviation = 0.1;
   else {
     block->millimeters = sqrt(
       #ifdef COREXY
-        square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD])
+        square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS])
       #else
-        square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS])
+        square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])
       #endif
-      + square(delta_mm[Z_AXIS])
     );
   }
   float inverse_millimeters = 1.0 / block->millimeters;  // Inverse millimeters to remove multiple divides