From 750acb41bc3312d31140349e4b494394a133379c Mon Sep 17 00:00:00 2001
From: Phil Hord <phil.hord@technicolor.com>
Date: Thu, 19 May 2016 14:45:51 -0400
Subject: [PATCH] Simulator: calculate position for corexy correctly

CoreXY turns the X and Y motors to render a target position differently
than straight cartesian printer does.  From the theory page on corexy.com,
where the motors are called A and B instead of X and Y:

    dx = 1/2(dA + dB), dY = 1/2(dA - dB)
    dA = dX + dY
    dB = dX - dY

Accordingly, each step of a single motor results in half of a step in the
X or Y axis.  To simplify this and not lose steps, make the pos[] array
hold 2*steps instead of single steps.  Adjust back to single steps with
/2 where needed.  Store 2*steps whenever writing to pos[] variables
which are not coreXY driven.

Since each step of X or Y (A or B) affects both X and Y position, send
updates to record_pin for all axes instead of only the "affected" axis.
The function record_pin will ignore reports for pins which did not change
from the previous call.  This also helps us keep from reporting duplicate
positions for half-steps in coreXY mode, too.
---
 simulator/simulator.c | 30 +++++++++++++++++++++++++-----
 1 file changed, 25 insertions(+), 5 deletions(-)

diff --git a/simulator/simulator.c b/simulator/simulator.c
index 54b3c5f..db8de4f 100644
--- a/simulator/simulator.c
+++ b/simulator/simulator.c
@@ -304,7 +304,7 @@ void cli(void) {
 #define in  false
 
 enum { X_AXIS, Y_AXIS, Z_AXIS, E_AXIS , AXIS_MAX , AXIS_NONE };
-static int pos[AXIS_MAX];            ///< Current position in steps
+static int pos[AXIS_MAX];            ///< Current position in 2 * steps
 
 static bool direction[PIN_NB];
 static bool state[PIN_NB];
@@ -314,7 +314,7 @@ static void print_pos(void) {
   int i;
   if (trace_pos) {
     for ( i = X_AXIS ; i < AXIS_MAX ; i++ ) {
-      sim_info_cont("%c:%5d   ", axis[i], pos[i]);
+      sim_info_cont("%c:%5d   ", axis[i], pos[i] / 2);
     }
     if (verbose > 1)
       clearline();
@@ -395,9 +395,29 @@ void _WRITE(pin_t pin, bool s) {
     default:
       break;
     }
+    switch ( axis ) {
+      #ifdef KINEMATICS_COREXY
+        case X_AXIS:
+          pos[X_AXIS] += dir;
+          pos[Y_AXIS] += dir;
+          break;
+        case Y_AXIS:
+          pos[X_AXIS] += dir;
+          pos[Y_AXIS] -= dir;
+          break;
+      #endif
+      case Z_AXIS:
+      case E_AXIS:
+      default:
+        pos[axis] += 2 * dir;
+        break;
+      case AXIS_NONE:
+        break;
+    }
     if ( axis != AXIS_NONE ) {
-      pos[axis] += dir;
-      record_pin(TRACE_POS + axis, pos[axis], nseconds);
+      for (int a = X_AXIS; a <= E_AXIS; a++)
+        record_pin(TRACE_POS + axis, pos[axis] / 2, nseconds);
+
       print_pos();
 
       for (int a = X_AXIS; a < E_AXIS; a++)
@@ -413,7 +433,7 @@ static void sim_endstop( int axis ) {
   bool on ;
 
   if (axis == AXIS_NONE)        return;
-  else if (pos[axis] <= -10)    on = true;
+  else if (pos[axis] <= -20)    on = true;
   else if (pos[axis] >= 0)      on = false;
   else                          return ;