From 725165787224e386c05f80a66a73d212a65712d2 Mon Sep 17 00:00:00 2001
From: chamnit <chamnit@Masamune.local>
Date: Mon, 25 Jul 2016 11:11:27 -0600
Subject: [PATCH 1/2] CoreXY homing fix attempt.

---
 grbl/config.h  |  2 +-
 grbl/limits.c  | 40 +++++++++++++++++++++++++++++-----------
 grbl/planner.c |  8 ++++----
 grbl/system.c  | 23 ++++++++++++++++++-----
 grbl/system.h  |  3 +++
 5 files changed, 55 insertions(+), 21 deletions(-)

diff --git a/grbl/config.h b/grbl/config.h
index bd9c0d6..d524796 100644
--- a/grbl/config.h
+++ b/grbl/config.h
@@ -149,7 +149,7 @@
 // defined at (http://corexy.com/theory.html). Motors are assumed to positioned and wired exactly as
 // described, if not, motions may move in strange directions. Grbl assumes the CoreXY A and B motors
 // have the same steps per mm internally.
-// #define COREXY // Default disabled. Uncomment to enable.
+#define COREXY // Default disabled. Uncomment to enable.
 
 // Inverts pin logic of the control command pins. This essentially means when this option is enabled
 // you can use normally-closed switches, rather than the default normally-open switches.
diff --git a/grbl/limits.c b/grbl/limits.c
index 6cd1cd9..947a6ed 100644
--- a/grbl/limits.c
+++ b/grbl/limits.c
@@ -184,7 +184,21 @@ void limits_go_home(uint8_t cycle_mask)
       // Set target location for active axes and setup computation for homing rate.
       if (bit_istrue(cycle_mask,bit(idx))) {
         n_active_axis++;
-        sys.position[idx] = 0;
+        #ifdef COREXY
+          int32_t axis_position;
+          if (idx == X_AXIS) {
+            axis_position = system_convert_corexy_to_x_axis_steps(sys.position);
+            sys.position[A_MOTOR] = axis_position;
+            sys.position[B_MOTOR] = -axis_position;
+          } else if (idx == Y_AXIS) {
+            axis_position = system_convert_corexy_to_y_axis_steps(sys.position);
+            sys.position[A_MOTOR] = sys.position[B_MOTOR] = axis_position;
+          } else { 
+            sys.position[Z_AXIS] = 0; 
+          }
+        #else
+          sys.position[idx] = 0;
+        #endif
         // Set target direction based on cycle mask and homing cycle approach state.
         // NOTE: This happens to compile smaller than any other implementation tried.
         if (bit_istrue(settings.homing_dir_mask,bit(idx))) {
@@ -219,7 +233,14 @@ void limits_go_home(uint8_t cycle_mask)
         limit_state = limits_get_state();
         for (idx=0; idx<N_AXIS; idx++) {
           if (axislock & step_pin[idx]) {
-            if (limit_state & (1 << idx)) { axislock &= ~(step_pin[idx]); }
+            if (limit_state & (1 << idx)) { 
+              #ifdef COREXY
+                if (idx==Z_AXIS) { axislock &= ~(step_pin[Z_AXIS]); }
+                else { axislock &= ~(step_pin[A_MOTOR]|step_pin[B_MOTOR]); }
+              #else
+                axislock &= ~(step_pin[idx]); 
+              #endif
+            }
           }
         }
         sys.homing_axis_lock = axislock;
@@ -270,9 +291,6 @@ void limits_go_home(uint8_t cycle_mask)
   // set up pull-off maneuver from axes limit switches that have been homed. This provides
   // some initial clearance off the switches and should also help prevent them from falsely
   // triggering when hard limits are enabled or when more than one axes shares a limit pin.
-  #ifdef COREXY
-    int32_t off_axis_position = 0;
-  #endif
   int32_t set_axis_position;
   // Set machine positions for homed limit switches. Don't update non-homed axes.
   for (idx=0; idx<N_AXIS; idx++) {
@@ -290,13 +308,13 @@ void limits_go_home(uint8_t cycle_mask)
       
       #ifdef COREXY
         if (idx==X_AXIS) { 
-          off_axis_position = (sys.position[B_MOTOR] - sys.position[A_MOTOR])/2;
-          sys.position[A_MOTOR] = set_axis_position - off_axis_position;
-          sys.position[B_MOTOR] = set_axis_position + off_axis_position;          
+          int32_t off_axis_position = system_convert_corexy_to_y_axis_steps(sys.position);
+          sys.position[A_MOTOR] = set_axis_position + off_axis_position;
+          sys.position[B_MOTOR] = set_axis_position - off_axis_position;          
         } else if (idx==Y_AXIS) {
-          off_axis_position = (sys.position[A_MOTOR] + sys.position[B_MOTOR])/2;
-          sys.position[A_MOTOR] = off_axis_position - set_axis_position;
-          sys.position[B_MOTOR] = off_axis_position + set_axis_position;
+          int32_t off_axis_position = system_convert_corexy_to_x_axis_steps(sys.position);
+          sys.position[A_MOTOR] = off_axis_position + set_axis_position;
+          sys.position[B_MOTOR] = off_axis_position - set_axis_position;
         } else {
           sys.position[idx] = set_axis_position;
         }        
diff --git a/grbl/planner.c b/grbl/planner.c
index c0d769d..f1eda9e 100644
--- a/grbl/planner.c
+++ b/grbl/planner.c
@@ -424,10 +424,10 @@ void plan_sync_position()
   uint8_t idx;
   for (idx=0; idx<N_AXIS; idx++) {
     #ifdef COREXY
-     if (idx==A_MOTOR) { 
-        pl.position[idx] = (sys.position[A_MOTOR] + sys.position[B_MOTOR])/2;
-      } else if (idx==B_MOTOR) { 
-        pl.position[idx] = (sys.position[A_MOTOR] - sys.position[B_MOTOR])/2;
+     if (idx==X_AXIS) { 
+        pl.position[X_AXIS] = system_convert_corexy_to_x_axis_steps(sys.position);
+      } else if (idx==Y_AXIS) { 
+        pl.position[Y_AXIS] = system_convert_corexy_to_y_axis_steps(sys.position);
       } else {
         pl.position[idx] = sys.position[idx];
       }
diff --git a/grbl/system.c b/grbl/system.c
index b3371d9..ff678f7 100644
--- a/grbl/system.c
+++ b/grbl/system.c
@@ -264,10 +264,10 @@ float system_convert_axis_steps_to_mpos(int32_t *steps, uint8_t idx)
 {
   float pos;
   #ifdef COREXY
-    if (idx==A_MOTOR) { 
-      pos = 0.5*((steps[A_MOTOR] + steps[B_MOTOR])/settings.steps_per_mm[idx]);
-    } else if (idx==B_MOTOR) {
-      pos = 0.5*((steps[A_MOTOR] - steps[B_MOTOR])/settings.steps_per_mm[idx]);
+    if (idx==X_AXIS) { 
+      pos = (float)system_convert_corexy_to_x_axis_steps(steps) / settings.steps_per_mm[idx];
+    } else if (idx==Y_AXIS) {
+      pos = (float)system_convert_corexy_to_y_axis_steps(steps) / settings.steps_per_mm[idx];
     } else {
       pos = steps[idx]/settings.steps_per_mm[idx];
     }
@@ -276,7 +276,7 @@ float system_convert_axis_steps_to_mpos(int32_t *steps, uint8_t idx)
   #endif
   return(pos);
 }
-  
+
 
 void system_convert_array_steps_to_mpos(float *position, int32_t *steps)
 {
@@ -286,3 +286,16 @@ void system_convert_array_steps_to_mpos(float *position, int32_t *steps)
   }
   return;
 }
+
+
+int32_t system_convert_corexy_to_x_axis_steps(int32_t *steps)
+{
+  return( (steps[A_MOTOR] + steps[B_MOTOR])/2 );
+}
+
+
+int32_t system_convert_corexy_to_y_axis_steps(int32_t *steps)
+{
+  return( (steps[A_MOTOR] - steps[B_MOTOR])/2 );
+}
+
diff --git a/grbl/system.h b/grbl/system.h
index 079d68f..6774d54 100644
--- a/grbl/system.h
+++ b/grbl/system.h
@@ -106,4 +106,7 @@ float system_convert_axis_steps_to_mpos(int32_t *steps, uint8_t idx);
 // Updates a machine 'position' array based on the 'step' array sent.
 void system_convert_array_steps_to_mpos(float *position, int32_t *steps);
 
+int32_t system_convert_corexy_to_x_axis_steps(int32_t *steps);
+int32_t system_convert_corexy_to_y_axis_steps(int32_t *steps);
+
 #endif

From 59a4d2ef06bc5ce09f6d38735f4638b1a63642e7 Mon Sep 17 00:00:00 2001
From: chamnit <chamnit@Masamune.local>
Date: Mon, 25 Jul 2016 13:50:08 -0600
Subject: [PATCH 2/2] CoreXY homing bug fix attempt 2.

---
 grbl/config.h  | 4 ++--
 grbl/limits.c  | 7 +++----
 grbl/planner.c | 4 ++--
 grbl/system.c  | 3 +--
 grbl/system.h  | 1 +
 5 files changed, 9 insertions(+), 10 deletions(-)

diff --git a/grbl/config.h b/grbl/config.h
index d524796..f01935b 100644
--- a/grbl/config.h
+++ b/grbl/config.h
@@ -72,8 +72,8 @@
 // on separate pin, but homed in one cycle. Also, it should be noted that the function of hard limits 
 // will not be affected by pin sharing.
 // NOTE: Defaults are set for a traditional 3-axis CNC machine. Z-axis first to clear, followed by X & Y.
-#define HOMING_CYCLE_0 (1<<Z_AXIS)                // REQUIRED: First move Z to clear workspace.
-#define HOMING_CYCLE_1 ((1<<X_AXIS)|(1<<Y_AXIS))  // OPTIONAL: Then move X,Y at the same time.
+#define HOMING_CYCLE_0 (1<<X_AXIS)                // REQUIRED: First move Z to clear workspace.
+#define HOMING_CYCLE_1 (1<<Y_AXIS)  // OPTIONAL: Then move X,Y at the same time.
 // #define HOMING_CYCLE_2                         // OPTIONAL: Uncomment and add axes mask to enable
 
 // Number of homing cycles performed after when the machine initially jogs to limit switches.
diff --git a/grbl/limits.c b/grbl/limits.c
index 947a6ed..47e70bf 100644
--- a/grbl/limits.c
+++ b/grbl/limits.c
@@ -185,13 +185,12 @@ void limits_go_home(uint8_t cycle_mask)
       if (bit_istrue(cycle_mask,bit(idx))) {
         n_active_axis++;
         #ifdef COREXY
-          int32_t axis_position;
           if (idx == X_AXIS) {
-            axis_position = system_convert_corexy_to_x_axis_steps(sys.position);
+            int32_t axis_position = system_convert_corexy_to_y_axis_steps(sys.position);
             sys.position[A_MOTOR] = axis_position;
             sys.position[B_MOTOR] = -axis_position;
           } else if (idx == Y_AXIS) {
-            axis_position = system_convert_corexy_to_y_axis_steps(sys.position);
+            int32_t axis_position = system_convert_corexy_to_x_axis_steps(sys.position);
             sys.position[A_MOTOR] = sys.position[B_MOTOR] = axis_position;
           } else { 
             sys.position[Z_AXIS] = 0; 
@@ -306,7 +305,7 @@ void limits_go_home(uint8_t cycle_mask)
         }
       #endif
       
-      #ifdef COREXY
+      #ifdef COREXY    
         if (idx==X_AXIS) { 
           int32_t off_axis_position = system_convert_corexy_to_y_axis_steps(sys.position);
           sys.position[A_MOTOR] = set_axis_position + off_axis_position;
diff --git a/grbl/planner.c b/grbl/planner.c
index f1eda9e..5b54410 100644
--- a/grbl/planner.c
+++ b/grbl/planner.c
@@ -292,9 +292,9 @@ uint8_t plan_check_full_buffer()
       }
       block->step_event_count = max(block->step_event_count, block->steps[idx]);
       if (idx == A_MOTOR) {
-        delta_mm = ((target_steps[X_AXIS]-pl.position[X_AXIS]) + (target_steps[Y_AXIS]-pl.position[Y_AXIS]))/settings.steps_per_mm[idx];
+        delta_mm = (target_steps[X_AXIS]-pl.position[X_AXIS] + target_steps[Y_AXIS]-pl.position[Y_AXIS])/settings.steps_per_mm[idx];
       } else if (idx == B_MOTOR) {
-        delta_mm = ((target_steps[X_AXIS]-pl.position[X_AXIS]) - (target_steps[Y_AXIS]-pl.position[Y_AXIS]))/settings.steps_per_mm[idx];
+        delta_mm = (target_steps[X_AXIS]-pl.position[X_AXIS] - target_steps[Y_AXIS]+pl.position[Y_AXIS])/settings.steps_per_mm[idx];
       } else {
         delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx];
       }
diff --git a/grbl/system.c b/grbl/system.c
index ff678f7..f16b8e8 100644
--- a/grbl/system.c
+++ b/grbl/system.c
@@ -288,12 +288,11 @@ void system_convert_array_steps_to_mpos(float *position, int32_t *steps)
 }
 
 
+// CoreXY calculation only. Returns x or y-axis "steps" based on CoreXY motor steps.
 int32_t system_convert_corexy_to_x_axis_steps(int32_t *steps)
 {
   return( (steps[A_MOTOR] + steps[B_MOTOR])/2 );
 }
-
-
 int32_t system_convert_corexy_to_y_axis_steps(int32_t *steps)
 {
   return( (steps[A_MOTOR] - steps[B_MOTOR])/2 );
diff --git a/grbl/system.h b/grbl/system.h
index 6774d54..09a3171 100644
--- a/grbl/system.h
+++ b/grbl/system.h
@@ -106,6 +106,7 @@ float system_convert_axis_steps_to_mpos(int32_t *steps, uint8_t idx);
 // Updates a machine 'position' array based on the 'step' array sent.
 void system_convert_array_steps_to_mpos(float *position, int32_t *steps);
 
+// CoreXY calculation only. Returns x or y-axis "steps" based on CoreXY motor steps.
 int32_t system_convert_corexy_to_x_axis_steps(int32_t *steps);
 int32_t system_convert_corexy_to_y_axis_steps(int32_t *steps);