Introduce concept of homed or not

  add G28.6 to display homed status
  add G28.5 to clear homed status (set unhomed ala linuxcnc)

M306 now requires axis be homed or returns error, and forces axis to be homed before setting it again
 Note M306 may still be buggy if compenstaion transform is on

diff --git a/src/libs/Kernel.cpp b/src/libs/Kernel.cpp
index 20fef96..0538cb4 100644 (file)
--- a/src/libs/Kernel.cpp
+++ b/src/libs/Kernel.cpp
@@ -183,16 +183,8 @@ std::string Kernel::get_query_string()
     }
 
     if(running) {
-        // get real time current actuator position in mm
-        ActuatorCoordinates current_position{
-            robot->actuators[X_AXIS]->get_current_position(),
-            robot->actuators[Y_AXIS]->get_current_position(),
-            robot->actuators[Z_AXIS]->get_current_position()
-        };
-
-        // get machine position from the actuator position using FK
         float mpos[3];
-        robot->arm_solution->actuator_to_cartesian(current_position, mpos);
+        robot->get_current_machine_position(mpos);
         // current_position/mpos includes the compensation transform so we need to get the inverse to get actual position
         if(robot->compensationTransform) robot->compensationTransform(mpos, true); // get inverse compensation transform
 

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 3a2a86c..a4ae40c 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -292,6 +292,19 @@ std::vector<Robot::wcs_t> Robot::get_wcs_state() const
     return v;
 }
 
+void Robot::get_current_machine_position(float *pos) const
+{
+    // get real time current actuator position in mm
+    ActuatorCoordinates current_position{
+        actuators[X_AXIS]->get_current_position(),
+        actuators[Y_AXIS]->get_current_position(),
+        actuators[Z_AXIS]->get_current_position()
+    };
+
+    // get machine position from the actuator position using FK
+    arm_solution->actuator_to_cartesian(current_position, pos);
+}
+
 int Robot::print_position(uint8_t subcode, char *buf, size_t bufsize) const
 {
     // M114.1 is a new way to do this (similar to how GRBL does it).
@@ -315,27 +328,26 @@ int Robot::print_position(uint8_t subcode, char *buf, size_t bufsize) const
 
     } else {
         // get real time positions
-        // current actuator position in mm
-        ActuatorCoordinates current_position{
-            actuators[X_AXIS]->get_current_position(),
-            actuators[Y_AXIS]->get_current_position(),
-            actuators[Z_AXIS]->get_current_position()
-        };
-
-        // get machine position from the actuator position using FK
         float mpos[3];
-        arm_solution->actuator_to_cartesian(current_position, mpos);
+        get_current_machine_position(mpos);
+
         // current_position/mpos includes the compensation transform so we need to get the inverse to get actual position
         if(compensationTransform) compensationTransform(mpos, true); // get inverse compensation transform
 
         if(subcode == 1) { // M114.1 print realtime WCS
             wcs_t pos= mcs2wcs(mpos);
-            n = snprintf(buf, bufsize, "WPOS: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get<X_AXIS>(pos)), from_millimeters(std::get<Y_AXIS>(pos)), from_millimeters(std::get<Z_AXIS>(pos)));
+            n = snprintf(buf, bufsize, "WCS: X:%1.4f Y:%1.4f Z:%1.4f", from_millimeters(std::get<X_AXIS>(pos)), from_millimeters(std::get<Y_AXIS>(pos)), from_millimeters(std::get<Z_AXIS>(pos)));
 
         } else if(subcode == 2) { // M114.2 print realtime Machine coordinate system
-            n = snprintf(buf, bufsize, "MPOS: X:%1.4f Y:%1.4f Z:%1.4f", mpos[X_AXIS], mpos[Y_AXIS], mpos[Z_AXIS]);
+            n = snprintf(buf, bufsize, "MCS: X:%1.4f Y:%1.4f Z:%1.4f", mpos[X_AXIS], mpos[Y_AXIS], mpos[Z_AXIS]);
 
         } else if(subcode == 3) { // M114.3 print realtime actuator position
+            // get real time current actuator position in mm
+            ActuatorCoordinates current_position{
+                actuators[X_AXIS]->get_current_position(),
+                actuators[Y_AXIS]->get_current_position(),
+                actuators[Z_AXIS]->get_current_position()
+            };
             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]);
         }
     }
@@ -892,9 +904,9 @@ void Robot::reset_axis_position(float x, float y, float z)
 // Reset the position for an axis (used in homing, and to reset extruder after suspend)
 void Robot::reset_axis_position(float position, int axis)
 {
-    last_milestone[axis] = position;
+    last_machine_position[axis] = position;
     if(axis <= Z_AXIS) {
-        reset_axis_position(last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]);
+        reset_axis_position(last_machine_position[X_AXIS], last_machine_position[Y_AXIS], last_machine_position[Z_AXIS]);
 #if MAX_ROBOT_ACTUATORS > 3
     }else{
         // extruders need to be set not calculated
@@ -907,8 +919,9 @@ void Robot::reset_axis_position(float position, int axis)
 // then sets the axis positions to match. currently only called from Endstops.cpp and RotaryDeltaCalibration.cpp
 void Robot::reset_actuator_position(const ActuatorCoordinates &ac)
 {
-    for (size_t i = X_AXIS; i <= Z_AXIS; i++)
-        actuators[i]->change_last_milestone(ac[i]);
+    for (size_t i = X_AXIS; i <= Z_AXIS; i++) {
+        if(!isnan(ac[i])) actuators[i]->change_last_milestone(ac[i]);
+    }
 
     // now correct axis positions then recorrect actuator to account for rounding
     reset_position_from_current_actuator_position();

diff --git a/src/modules/robot/Robot.h b/src/modules/robot/Robot.h
index 336b3ee..736897c 100644 (file)
--- a/src/modules/robot/Robot.h
+++ b/src/modules/robot/Robot.h
@@ -53,6 +53,7 @@ class Robot : public Module {
         float get_axis_position(int axis) const { return(this->last_milestone[axis]); }
         void get_axis_position(float position[], size_t n= N_PRIMARY_AXIS) const { memcpy(position, this->last_milestone, n*sizeof(float)); }
         wcs_t get_axis_position() const { return wcs_t(last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]); }
+        void get_current_machine_position(float *pos) const;
         int print_position(uint8_t subcode, char *buf, size_t bufsize) const;
         uint8_t get_current_wcs() const { return current_wcs; }
         std::vector<wcs_t> get_wcs_state() const;

diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp
index 1193dc8..23749c3 100644 (file)
--- a/src/modules/tools/endstops/Endstops.cpp
+++ b/src/modules/tools/endstops/Endstops.cpp
@@ -130,6 +130,7 @@ Endstops::Endstops()
     this->status = NOT_HOMING;
     home_offset[0] = home_offset[1] = home_offset[2] = 0.0F;
     debounce.fill(0);
+    homed.reset();
 }
 
 void Endstops::on_module_loaded()
@@ -480,7 +481,7 @@ void Endstops::home(std::bitset<3> a)
     // reset debounce counts
     debounce.fill(0);
 
-    // turn off any compensation transform
+    // turn off any compensation transform so Z does not move as XY home
     auto savect= THEROBOT->compensationTransform;
     THEROBOT->compensationTransform= nullptr;
 
@@ -548,6 +549,11 @@ void Endstops::home(std::bitset<3> a)
     // restore compensationTransform
     THEROBOT->compensationTransform= savect;
 
+    // set flag indicating axis was homed, it stays set once set until H/W reset or unhomed
+    if(!homed[X_AXIS] && axis_to_home[X_AXIS]) homed.set(X_AXIS);
+    if(!homed[Y_AXIS] && axis_to_home[Y_AXIS]) homed.set(Y_AXIS);
+    if(!homed[Z_AXIS] && axis_to_home[Z_AXIS]) homed.set(Z_AXIS);
+
     this->status = NOT_HOMING;
 }
 
@@ -585,23 +591,41 @@ void Endstops::process_home_command(Gcode* gcode)
     } else if(gcode->subcode == 3) { // G28.3 is a smoothie special it sets manual homing
         if(gcode->get_num_args() == 0) {
             THEROBOT->reset_axis_position(0, 0, 0);
+            homed.set();
         } else {
             // do a manual homing based on given coordinates, no endstops required
-            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('X')){ THEROBOT->reset_axis_position(gcode->get_value('X'), X_AXIS); homed.set(X_AXIS); }
+            if(gcode->has_letter('Y')){ THEROBOT->reset_axis_position(gcode->get_value('Y'), Y_AXIS); homed.set(Y_AXIS); }
+            if(gcode->has_letter('Z')){ THEROBOT->reset_axis_position(gcode->get_value('Z'), Z_AXIS); homed.set(Z_AXIS); }
         }
         return;
 
     } else if(gcode->subcode == 4) { // G28.4 is a smoothie special it sets manual homing based on the actuator position (used for rotary delta)
         // do a manual homing based on given coordinates, no endstops required
-        ActuatorCoordinates ac;
-        if(gcode->has_letter('X')) ac[0] =  gcode->get_value('X');
-        if(gcode->has_letter('Y')) ac[1] =  gcode->get_value('Y');
-        if(gcode->has_letter('Z')) ac[2] =  gcode->get_value('Z');
+        ActuatorCoordinates ac{NAN, NAN, NAN};
+        if(gcode->has_letter('X')){ ac[0] =  gcode->get_value('X'); homed.set(X_AXIS); }
+        if(gcode->has_letter('Y')){ ac[1] =  gcode->get_value('Y'); homed.set(Y_AXIS); }
+        if(gcode->has_letter('Z')){ ac[2] =  gcode->get_value('Z'); homed.set(Z_AXIS); }
         THEROBOT->reset_actuator_position(ac);
         return;
 
+    } else if(gcode->subcode == 5) { // G28.5 is a smoothie special it clears the homed flag for the specified axis, or all if not specifed
+        if(gcode->get_num_args() == 0) {
+            homed.reset();
+        } else {
+            if(gcode->has_letter('X')) homed.reset(X_AXIS);
+            if(gcode->has_letter('Y')) homed.reset(Y_AXIS);
+            if(gcode->has_letter('Z')) homed.reset(Z_AXIS);
+        }
+        return;
+
+    } else if(gcode->subcode == 6) { // G28.6 is a smoothie special it shows the homing status of each axis
+        for (int i = 0; i < 3; ++i) {
+            gcode->stream->printf("%c:%d ", 'X'+i, homed.test(i));
+        }
+        gcode->add_nl= true;
+        return;
+
     } else if(THEKERNEL->is_grbl_mode()) {
         gcode->stream->printf("error:Unsupported command\n");
         return;
@@ -673,6 +697,7 @@ void Endstops::process_home_command(Gcode* gcode)
         if(!THEKERNEL->is_grbl_mode()) {
             THEKERNEL->streams->printf("Homing cycle aborted by kill\n");
         }
+        homed.reset();
         return;
     }
 
@@ -747,19 +772,35 @@ void Endstops::process_home_command(Gcode* gcode)
 void Endstops::set_homing_offset(Gcode *gcode)
 {
     // Similar to M206 but sets Homing offsets based on current position
+    // We want to set the machine position to what we specify, but the current position of the actuators is after the compensation transform
+    // we do not want to reset the actuator positions but we do want to make the current position read Z0 in machine coordinates after homing.
+    // TODO so what do we set the homing offset to to make that happen?
     float mpos[3];
-    THEROBOT->get_axis_position(mpos);    // get machine position from robot
-    // add in the current home offset so we can set it multiple times without accumulating the error
-    for (int i = 0; i < 3; ++i)  mpos[i] += home_offset[i];
+    THEROBOT->get_current_machine_position(mpos);
 
     if (gcode->has_letter('X')) {
+        if(!homed[X_AXIS]) {
+            gcode->stream->printf("error: Axis X must be homed before setting Homing offset\n");
+            return;
+        }
         home_offset[0] += (THEROBOT->to_millimeters(gcode->get_value('X')) - mpos[X_AXIS]);
+        homed.reset(X_AXIS); // force it to be homed
     }
     if (gcode->has_letter('Y')) {
+        if(!homed[Y_AXIS]) {
+            gcode->stream->printf("error: Axis Y must be homed before setting Homing offset\n");
+            return;
+        }
         home_offset[1] += (THEROBOT->to_millimeters(gcode->get_value('Y')) - mpos[Y_AXIS]);
+        homed.reset(Y_AXIS); // force it to be homed
     }
     if (gcode->has_letter('Z')) {
+        if(!homed[Z_AXIS]) {
+            gcode->stream->printf("error: Axis Z must be homed before setting Homing offset\n");
+            return;
+        }
         home_offset[2] += (THEROBOT->to_millimeters(gcode->get_value('Z')) - mpos[Z_AXIS]);
+        homed.reset(Z_AXIS); // force it to be homed
     }
 
     gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f will take effect next home\n", home_offset[0], home_offset[1], home_offset[2]);

diff --git a/src/modules/tools/endstops/Endstops.h b/src/modules/tools/endstops/Endstops.h
index 387781b..3ad1444 100644 (file)
--- a/src/modules/tools/endstops/Endstops.h
+++ b/src/modules/tools/endstops/Endstops.h
@@ -53,6 +53,7 @@ class Endstops : public Module{
         std::bitset<3> home_direction;
         std::bitset<3> limit_enable;
         std::bitset<3> axis_to_home;
+        std::bitset<3> homed;
 
         struct {
             uint8_t homing_order:6;

diff --git a/src/modules/utils/panel/screens/cnc/DirectJogScreen.cpp b/src/modules/utils/panel/screens/cnc/DirectJogScreen.cpp
index 1d4cc28..6e035d0 100644 (file)
--- a/src/modules/utils/panel/screens/cnc/DirectJogScreen.cpp
+++ b/src/modules/utils/panel/screens/cnc/DirectJogScreen.cpp
@@ -136,16 +136,9 @@ void DirectJogScreen::on_exit()
 
 void DirectJogScreen::get_actuator_pos()
 {
-        // get real time positions
-        ActuatorCoordinates current_position{
-            THEROBOT->actuators[X_AXIS]->get_current_position(),
-            THEROBOT->actuators[Y_AXIS]->get_current_position(),
-            THEROBOT->actuators[Z_AXIS]->get_current_position()
-        };
-
         // get machine position from the actuator position using FK
         float mpos[3];
-        THEROBOT->arm_solution->actuator_to_cartesian(current_position, mpos);
+        THEROBOT->get_current_machine_position(mpos);
         Robot::wcs_t wpos= THEROBOT->mcs2wcs(mpos);
         this->pos[0]= THEROBOT->from_millimeters(std::get<X_AXIS>(wpos));
         this->pos[1]= THEROBOT->from_millimeters(std::get<Y_AXIS>(wpos));

diff --git a/src/modules/utils/panel/screens/cnc/WatchScreen.cpp b/src/modules/utils/panel/screens/cnc/WatchScreen.cpp
index 84e3a09..b413b53 100644 (file)
--- a/src/modules/utils/panel/screens/cnc/WatchScreen.cpp
+++ b/src/modules/utils/panel/screens/cnc/WatchScreen.cpp
@@ -110,16 +110,8 @@ void WatchScreen::on_refresh()
 void WatchScreen::get_wpos()
 {
     // get real time positions
-    // current actuator position in mm
-    ActuatorCoordinates current_position{
-        THEROBOT->actuators[X_AXIS]->get_current_position(),
-        THEROBOT->actuators[Y_AXIS]->get_current_position(),
-        THEROBOT->actuators[Z_AXIS]->get_current_position()
-    };
-
-    // get machine position from the actuator position using FK
     float mpos[3];
-    THEROBOT->arm_solution->actuator_to_cartesian(current_position, mpos);
+    THEROBOT->get_current_machine_position(mpos);
     Robot::wcs_t wpos= THEROBOT->mcs2wcs(mpos);
     this->wpos[0]= THEROBOT->from_millimeters(std::get<X_AXIS>(wpos));
     this->wpos[1]= THEROBOT->from_millimeters(std::get<Y_AXIS>(wpos));