Implement COREXZ in stepper.cpp and planner.cpp

Marlin/planner.cpp

@@ -542,6 +542,11 @@ float junction_deviation = 0.1;
     block->steps[A_AXIS] = labs(dx + dy);
     block->steps[B_AXIS] = labs(dx - dy);
     block->steps[Z_AXIS] = labs(dz);
+  #elif defined(COREXZ)
+    // corexz planning
+    block->steps[A_AXIS] = labs(dx + dz);
+    block->steps[Y_AXIS] = labs(dy);
+    block->steps[C_AXIS] = labs(dx - dz);
   #else
     // default non-h-bot planning
     block->steps[X_AXIS] = labs(dx);
@@ -572,6 +577,12 @@ float junction_deviation = 0.1;
     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
+  #elif defined(COREXZ)
+    if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
+    if (dy < 0) db |= BIT(Y_AXIS);
+    if (dz < 0) db |= BIT(Z_HEAD); // ...and Z
+    if (dx + dz < 0) db |= BIT(A_AXIS); // Motor A direction
+    if (dx - dz < 0) db |= BIT(C_AXIS); // Motor B direction
   #else
     if (dx < 0) db |= BIT(X_AXIS);
     if (dy < 0) db |= BIT(Y_AXIS); 
@@ -591,6 +602,11 @@ float junction_deviation = 0.1;
     #ifndef Z_LATE_ENABLE
       if (block->steps[Z_AXIS]) enable_z();
     #endif
+  #elif defined(COREXZ)
+    if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
+      enable_x();
+      enable_z();
+    }
   #else
     if (block->steps[X_AXIS]) enable_x();
     if (block->steps[Y_AXIS]) enable_y();
@@ -683,6 +699,13 @@ float junction_deviation = 0.1;
     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];
+  #elif defined(COREXZ)
+    float delta_mm[6];
+    delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
+    delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
+    delta_mm[Z_HEAD] = dz / axis_steps_per_unit[C_AXIS];
+    delta_mm[A_AXIS] = (dx + dz) / axis_steps_per_unit[A_AXIS];
+    delta_mm[C_AXIS] = (dx - dz) / axis_steps_per_unit[C_AXIS];
   #else
     float delta_mm[4];
     delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS];
@@ -698,6 +721,8 @@ float junction_deviation = 0.1;
     block->millimeters = sqrt(
       #ifdef COREXY
         square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS])
+      #elif defined(COREXZ)
+        square(delta_mm[X_HEAD]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_HEAD])
       #else
         square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])
       #endif

Marlin/stepper.cpp

@@ -342,34 +342,38 @@ FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
   return timer;
 }
 
-// set the stepper direction of each axis
+/**
+ * Set the stepper direction of each axis
+ *
+ *   X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY
+ *   X_AXIS=A_AXIS and Z_AXIS=C_AXIS for COREXZ
+ */
 void set_stepper_direction() {
 
-  // Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY)
+  if (TEST(out_bits, X_AXIS)) { // A_AXIS
-  if (TEST(out_bits, X_AXIS)) {
+    X_APPLY_DIR(INVERT_X_DIR, 0);
-    X_APPLY_DIR(INVERT_X_DIR,0);
     count_direction[X_AXIS] = -1;
   }
   else {
-    X_APPLY_DIR(!INVERT_X_DIR,0);
+    X_APPLY_DIR(!INVERT_X_DIR, 0);
     count_direction[X_AXIS] = 1;
   }
 
-  if (TEST(out_bits, Y_AXIS)) {
+  if (TEST(out_bits, Y_AXIS)) { // B_AXIS
-    Y_APPLY_DIR(INVERT_Y_DIR,0);
+    Y_APPLY_DIR(INVERT_Y_DIR, 0);
     count_direction[Y_AXIS] = -1;
   }
   else {
-    Y_APPLY_DIR(!INVERT_Y_DIR,0);
+    Y_APPLY_DIR(!INVERT_Y_DIR, 0);
     count_direction[Y_AXIS] = 1;
   }
   
-  if (TEST(out_bits, Z_AXIS)) {
+  if (TEST(out_bits, Z_AXIS)) { // C_AXIS
-    Z_APPLY_DIR(INVERT_Z_DIR,0);
+    Z_APPLY_DIR(INVERT_Z_DIR, 0);
     count_direction[Z_AXIS] = -1;
   }
   else {
-    Z_APPLY_DIR(!INVERT_Z_DIR,0);
+    Z_APPLY_DIR(!INVERT_Z_DIR, 0);
     count_direction[Z_AXIS] = 1;
   }
   
@@ -503,6 +507,11 @@ ISR(TIMER1_COMPA_vect) {
         // If DeltaX == -DeltaY, the movement is only in Y axis
         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))
+      #elif defined(COREXZ)
+        // Head direction in -X axis for CoreXZ bots.
+        // If DeltaX == -DeltaZ, the movement is only in Z axis
+        if ((current_block->steps[A_AXIS] != current_block->steps[C_AXIS]) || (TEST(out_bits, A_AXIS) == TEST(out_bits, C_AXIS))) {
+          if (TEST(out_bits, X_HEAD))
       #else
           if (TEST(out_bits, X_AXIS))   // stepping along -X axis (regular Cartesian bot)
       #endif
@@ -528,8 +537,11 @@ ISR(TIMER1_COMPA_vect) {
                 #endif
               }
           }
-      #ifdef COREXY
+      #if defined(COREXY) || defined(COREXZ)
         }
+      #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[A_AXIS] != current_block->steps[B_AXIS]) || (TEST(out_bits, A_AXIS) != TEST(out_bits, B_AXIS))) {
@@ -547,10 +559,19 @@ ISR(TIMER1_COMPA_vect) {
               UPDATE_ENDSTOP(Y, MAX);
             #endif
           }
-      #ifdef COREXY
+      #if defined(COREXY) || defined(COREXZ)
         }
       #endif
-      if (TEST(out_bits, Z_AXIS)) { // z -direction
+
+      #ifdef COREXZ
+        // Head direction in -Z axis for CoreXZ bots.
+        // If DeltaX == DeltaZ, the movement is only in X axis
+        if ((current_block->steps[A_AXIS] != current_block->steps[C_AXIS]) || (TEST(out_bits, A_AXIS) != TEST(out_bits, C_AXIS))) {
+          if (TEST(out_bits, Z_HEAD))
+      #else
+          if (TEST(out_bits, Z_AXIS))
+      #endif
+          { // z -direction
             #if HAS_Z_MIN
 
               #ifdef Z_DUAL_ENDSTOPS