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] 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