From 96ef08093c094dbcedb1b2c17083b06d92f65b88 Mon Sep 17 00:00:00 2001
From: Jim Morris <morris@wolfman.com>
Date: Thu, 28 Jul 2016 13:25:33 -0700
Subject: [PATCH] allow G28.3 to reset ABC print extra axis with get pos if
 they exist

---
 src/modules/robot/Robot.cpp             | 27 ++++++++++++++++++++-----
 src/modules/tools/endstops/Endstops.cpp |  3 +++
 2 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 2ae87fa0..4cb0c8be 100644
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -203,10 +203,11 @@ void Robot::load_config()
         Pin pins[3]; //step, dir, enable
         for (size_t i = 0; i < 3; i++) {
             pins[i].from_string(THEKERNEL->config->value(checksums[a][i])->by_default("nc")->as_string())->as_output();
-            if(!pins[i].connected()) {
-                if(i <= Z_AXIS) THEKERNEL->streams->printf("FATAL: motor %d is not defined in config\n", 'X'+a);
-                break; // if all pins are not defined then the axis is not defined (and axis need to be defined in contiguous order)
-            }
+        }
+
+        if(!pins[0].connected() || !pins[1].connected() || !pins[2].connected()) {
+            if(a <= Z_AXIS) THEKERNEL->streams->printf("FATAL: motor %d is not defined in config\n", 'X'+a);
+            break; // if any pin is not defined then the axis is not defined (and axis need to be defined in contiguous order)
         }
 
         StepperMotor *sm = new StepperMotor(pins[0], pins[1], pins[2]);
@@ -334,6 +335,21 @@ int Robot::print_position(uint8_t subcode, char *buf, size_t bufsize) const
             n = snprintf(buf, bufsize, "APOS: X:%1.4f Y:%1.4f Z:%1.4f", current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
         }
     }
+
+    #if MAX_ROBOT_ACTUATORS > 3
+    // deal with the ABC axis
+    for (int i = A_AXIS; i < n_motors; ++i) {
+        if(subcode == 4) { // M114.4 print last milestone
+            n += snprintf(&buf[n], bufsize-n, " %c:%1.4f", 'A'+i-A_AXIS, last_milestone[i]);
+
+        }else if(subcode == 3) { // M114.3 print actuator position
+            // current actuator position
+            float current_position= actuators[i]->get_current_position();
+            n += snprintf(&buf[n], bufsize-n, " %c:%1.4f", 'A'+i-A_AXIS, current_position);
+        }
+    }
+    #endif
+
     return n;
 }
 
@@ -894,9 +910,10 @@ void Robot::reset_axis_position(float position, int axis)
     if(axis <= Z_AXIS) {
         reset_axis_position(last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]);
 #if MAX_ROBOT_ACTUATORS > 3
-    }else{
+    }else if(axis < n_motors) {
         // extruders need to be set not calculated
         last_machine_position[axis]= position;
+        actuators[axis]->change_last_milestone(position);
 #endif
     }
 }
diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp
index aea7389f..3f3b78e2 100644
--- a/src/modules/tools/endstops/Endstops.cpp
+++ b/src/modules/tools/endstops/Endstops.cpp
@@ -583,6 +583,9 @@ void Endstops::process_home_command(Gcode* gcode)
             if(gcode->has_letter('X')) THEROBOT->reset_axis_position(gcode->get_value('X'), X_AXIS);
             if(gcode->has_letter('Y')) THEROBOT->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
             if(gcode->has_letter('Z')) THEROBOT->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
+            if(gcode->has_letter('A')) THEROBOT->reset_axis_position(gcode->get_value('A'), A_AXIS);
+            if(gcode->has_letter('B')) THEROBOT->reset_axis_position(gcode->get_value('B'), B_AXIS);
+            if(gcode->has_letter('C')) THEROBOT->reset_axis_position(gcode->get_value('C'), C_AXIS);
         }
         return;
 
-- 
2.20.1