From ded5de3e9dc2b59c79ac91fdf505a3c3ec9a7175 Mon Sep 17 00:00:00 2001
From: Rob Giseburt <giseburt@gmail.com>
Date: Tue, 14 Jan 2020 16:45:35 -0600
Subject: [PATCH] Fixes for join <-> axis mapping

---
 g2core/kinematics.cpp | 36 ++++++++++++++++--------------------
 g2core/stepper.cpp    |  5 ++---
 2 files changed, 18 insertions(+), 23 deletions(-)

diff --git a/g2core/kinematics.cpp b/g2core/kinematics.cpp
index bbe502d4..1999b27d 100644
--- a/g2core/kinematics.cpp
+++ b/g2core/kinematics.cpp
@@ -107,21 +107,17 @@ struct CartesianKinematics : KinematicsBase<axes, motors> {
     float steps_per_unit[motors];
     float offset[motors];
     bool needs_sync_encoders = true; // if true, we need to update the steps_offset
-    uint8_t joint_map[joints];  // for each joint, which motor or -1
+    int8_t motor_map[motors];  // for each motor, which joint it maps from
 
     float joint_position[joints];
 
-    void configure(const float new_steps_per_unit[motors], const int8_t motor_map[motors]) override
+    void configure(const float new_steps_per_unit[motors], const int8_t new_motor_map[motors]) override
     {
         for (uint8_t joint = 0; joint < joints; joint++) {
-            joint_map[joint] = -1;
             joint_position[joint] = 0;
         }
         for (uint8_t motor = 0; motor < motors; motor++) {
-            auto joint = motor_map[motor];
-            if (joint >= 0) {
-                joint_map[joint] = motor;
-            }
+            motor_map[motor] = new_motor_map[motor];
             steps_per_unit[motor] = new_steps_per_unit[motor];
         }
         needs_sync_encoders = true;
@@ -131,13 +127,14 @@ struct CartesianKinematics : KinematicsBase<axes, motors> {
                             const float end_velocity, const float segment_time, float steps[motors]) override
     {
         // joint == axis in cartesian kinematics
-        for (uint8_t joint = 0; joint < joints; joint++) {
-            uint8_t motor = joint_map[joint];
-            if (motor == -1) { continue; }
-
+        for (uint8_t motor = 0; motor < motors; motor++) {
+            int8_t joint = motor_map[motor];
+            if (joint == -1) {
+                continue;
+            }
             // if (needs_sync_encoders) {
-                // put the difference in offset
-                offset[motor] += (joint_position[joint] - position[joint]);
+            // put the difference in offset
+            offset[motor] += (joint_position[joint] - position[joint]);
             // }
 
             steps[motor] = (target[joint]+offset[motor]) * steps_per_unit[motor];
@@ -163,13 +160,12 @@ struct CartesianKinematics : KinematicsBase<axes, motors> {
         }
         for (uint8_t motor = 0; motor < motors; motor++) {
             best_steps_per_unit[motor] = -1.0;
-        }
+            int8_t joint = motor_map[motor];
+            if (joint == -1) {
+                continue;
+            }
 
-        // joint == motor in cartesian kinematics
-        for (uint8_t joint = 0; joint < joints; joint++) {
-            auto axis = joint; // cartesian, baby!
-            auto motor = joint_map[joint];
-            if (motor == -1) { continue; }
+            auto axis = joint; // it's cartesian, baby!
 
             // If this motor has a better (or the only) resolution, then we use this motor's value
             if (best_steps_per_unit[axis] < steps_per_unit[motor]) {
@@ -182,7 +178,7 @@ struct CartesianKinematics : KinematicsBase<axes, motors> {
             }
 
             joint_position[joint] = position[joint];
-        } // for joint
+        }
     }
 
     void sync_encoders() override { needs_sync_encoders = true; }
diff --git a/g2core/stepper.cpp b/g2core/stepper.cpp
index 33bc72cd..6ded213a 100644
--- a/g2core/stepper.cpp
+++ b/g2core/stepper.cpp
@@ -1006,12 +1006,11 @@ stat_t st_set_ma(nvObj_t *nv)
     uint8_t external_axis = nv->value_int;
 #if (AXES == 9)
     uint8_t remap_axis[9] = { 0,1,2,6,7,8,3,4,5 };
-#else
-    uint8_t remap_axis[6] = { 0,1,2,3,4,5 };
-#endif
     nv->value_int = remap_axis[nv->value_int];
+#endif
     ritorno(set_integer(nv, st_cfg.mot[_motor(nv->index)].motor_map, 0, AXES));
     nv->value_int = external_axis;
+    kn_config_changed();
     return(STAT_OK);
 }