Endstops set all axis positions at once if all axis homed (good for deltas)

Add probe offsets for probe position relative to head
set home z to probe offset after first probe and finding the bed

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 845b4f0..e688d9c 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -337,21 +337,18 @@ void Robot::on_gcode_received(void *argument)
             case 91: this->absolute_mode = false; gcode->mark_as_taken();  break;
             case 92: {
                 if(gcode->get_num_args() == 0) {
-                    clear_vector(this->last_milestone);
+                    for (int i = X_AXIS; i <= Z_AXIS; ++i) {
+                        reset_axis_position(0, i);
+                    }
+
                 } else {
                     for (char letter = 'X'; letter <= 'Z'; letter++) {
-                        if ( gcode->has_letter(letter) )
-                            this->last_milestone[letter - 'X'] = this->to_millimeters(gcode->get_value(letter));
+                        if ( gcode->has_letter(letter) ) {
+                            reset_axis_position(this->to_millimeters(gcode->get_value(letter)), letter - 'X');
+                        }
                     }
                 }
 
-                // TODO: handle any number of actuators
-                float actuator_pos[3];
-                arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
-
-                for (int i = 0; i < 3; i++)
-                    actuators[i]->change_last_milestone(actuator_pos[i]);
-
                 gcode->mark_as_taken();
                 return;
             }
@@ -559,13 +556,26 @@ void Robot::distance_in_gcode_is_known(Gcode *gcode)
     THEKERNEL->conveyor->append_gcode(gcode);
 }
 
-// Reset the position for all axes ( used in homing and G92 stuff )
+// reset the position for all axis (used in homing for delta as last_milestone may be bogus)
+void Robot::reset_axis_position(float x, float y, float z)
+{
+    this->last_milestone[X_AXIS] = x;
+    this->last_milestone[Y_AXIS] = y;
+    this->last_milestone[Z_AXIS] = z;
+
+    float actuator_pos[3];
+    arm_solution->cartesian_to_actuator(this->last_milestone, actuator_pos);
+    for (int i = 0; i < 3; i++)
+        actuators[i]->change_last_milestone(actuator_pos[i]);
+}
+
+// Reset the position for an axis (used in homing and G92)
 void Robot::reset_axis_position(float position, int axis)
 {
     this->last_milestone[axis] = position;
 
     float actuator_pos[3];
-    arm_solution->cartesian_to_actuator(last_milestone, actuator_pos);
+    arm_solution->cartesian_to_actuator(this->last_milestone, actuator_pos);
 
     for (int i = 0; i < 3; i++)
         actuators[i]->change_last_milestone(actuator_pos[i]);

diff --git a/src/modules/robot/Robot.h b/src/modules/robot/Robot.h
index 469cc22..1a1c5df 100644 (file)
--- a/src/modules/robot/Robot.h
+++ b/src/modules/robot/Robot.h
@@ -29,6 +29,7 @@ class Robot : public Module {
         void on_set_public_data(void* argument);
 
         void reset_axis_position(float position, int axis);
+        void reset_axis_position(float x, float y, float z);
         void get_axis_position(float position[]);
         float to_millimeters(float value);
         float from_millimeters(float value);

diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp
index 7b8662d..8ee22c5 100644 (file)
--- a/src/modules/tools/endstops/Endstops.cpp
+++ b/src/modules/tools/endstops/Endstops.cpp
@@ -562,10 +562,18 @@ void Endstops::on_gcode_received(void *argument)
                 home(axes_to_move);
             }
 
-            // Zero the ax(i/e)s position, add in the home offset
-            for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
-                if ( (axes_to_move >> c)  & 1 ) {
-                    THEKERNEL->robot->reset_axis_position(this->homing_position[c] + this->home_offset[c], c);
+            if(home_all) {
+                // for deltas this may be important rather than setting each individually
+                THEKERNEL->robot->reset_axis_position(
+                    this->homing_position[X_AXIS] + this->home_offset[X_AXIS],
+                    this->homing_position[Y_AXIS] + this->home_offset[Y_AXIS],
+                    this->homing_position[Z_AXIS] + this->home_offset[Z_AXIS]);
+            }else{
+                // Zero the ax(i/e)s position, add in the home offset
+                for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
+                    if ( (axes_to_move >> c)  & 1 ) {
+                        THEKERNEL->robot->reset_axis_position(this->homing_position[c] + this->home_offset[c], c);
+                    }
                 }
             }
 

diff --git a/src/modules/tools/zprobe/ThreePointStrategy.cpp b/src/modules/tools/zprobe/ThreePointStrategy.cpp
index 35cd6e9..ed3739e 100644 (file)
--- a/src/modules/tools/zprobe/ThreePointStrategy.cpp
+++ b/src/modules/tools/zprobe/ThreePointStrategy.cpp
@@ -11,6 +11,7 @@
 #include "Conveyor.h"
 #include "ZProbe.h"
 #include "Plane3D.h"
+#include "nuts_bolts.h"
 
 #include <string>
 #include <algorithm>
@@ -20,6 +21,7 @@
 #define probe_point_1_checksum       CHECKSUM("point1")
 #define probe_point_2_checksum       CHECKSUM("point2")
 #define probe_point_3_checksum       CHECKSUM("point3")
+#define probe_offsets_checksum       CHECKSUM("probe_offsets")
 #define home_checksum                CHECKSUM("home_first")
 #define tolerance_checksum           CHECKSUM("tolerance")
 
@@ -46,6 +48,10 @@ bool ThreePointStrategy::handleConfig()
     if(!p2.empty()) probe_points[1] = parseXY(p2.c_str());
     if(!p3.empty()) probe_points[2] = parseXY(p3.c_str());
 
+    // Probe offsets xxx,yyy,zzz
+    std::string po = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_offsets_checksum)->by_default("0,0,0")->as_string();
+    this->probe_offsets= parseXYZ(po.c_str());
+
     this->home= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, home_checksum)->by_default(true)->as_bool();
     this->tolerance= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, tolerance_checksum)->by_default(0.03F)->as_number();
     return true;
@@ -96,13 +102,25 @@ bool ThreePointStrategy::handleGcode(Gcode *gcode)
             THEKERNEL->robot->adjustZfnc= nullptr;
             return true;
 
+        } else if(gcode->m == 565) { // M565: Set Z probe offsets
+            float x= 0, y= 0, z= 0;
+            if(gcode->has_letter('X')) x = gcode->get_value('X');
+            if(gcode->has_letter('Y')) y = gcode->get_value('Y');
+            if(gcode->has_letter('Z')) z = gcode->get_value('Z');
+            probe_offsets = std::make_tuple(x, y, z);
+            return true;
+
         } else if(gcode->m == 503) {
+            float x, y, z;
             gcode->stream->printf(";Probe points:\n");
             for (int i = 0; i < 3; ++i) {
-                float x, y;
                 std::tie(x, y) = probe_points[i];
                 gcode->stream->printf("M557 P%d X%1.5f Y%1.5f\n", i, x, y);
             }
+            gcode->stream->printf(";Probe offsets:\n");
+            std::tie(x, y, z) = probe_offsets;
+            gcode->stream->printf("M565 X%1.5f Y%1.5f Z%1.5f\n", x, y, z);
+
             // TODO encode plane if set and M500
             return true;
 
@@ -157,15 +175,21 @@ bool ThreePointStrategy::doProbing(StreamOutput *stream)
 
     // move to the first probe point
     std::tie(x, y) = probe_points[0];
+    // offset by the probe XY offset
+    x -= std::get<X_AXIS>(this->probe_offsets);
+    y -= std::get<Y_AXIS>(this->probe_offsets);
     zprobe->coordinated_move(x, y, NAN, zprobe->getFastFeedrate());
 
     // for now we use probe to find bed and not the Z min endstop
+    // the first probe point becomes Z == 0 effectively so if we home Z or manually set z after this, it needs to be at the first probe point
+
     // TODO this needs to be configurable to use min z or probe
+    // TODO if using probe then we probably need to set Z to 0 at first probe point, but take into account probe offset from head
+    THEKERNEL->robot->reset_axis_position(std::get<Z_AXIS>(this->probe_offsets), Z_AXIS);
 
     // find bed via probe
     int s;
     if(!zprobe->run_probe(s, true)) return false;
-    // do we need to set set to Z == 0 here? as the rest is relative anyway
 
     // move up to specified probe start position
     zprobe->coordinated_move(NAN, NAN, zprobe->getProbeHeight(), zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed
@@ -174,15 +198,16 @@ bool ThreePointStrategy::doProbing(StreamOutput *stream)
     Vector3 v[3];
     for (int i = 0; i < 3; ++i) {
         std::tie(x, y) = probe_points[i];
-        float z = zprobe->probeDistance(x, y);
+        // offset moves by the probe XY offset
+        float z = zprobe->probeDistance(x-std::get<X_AXIS>(this->probe_offsets), y-std::get<Y_AXIS>(this->probe_offsets));
         if(isnan(z)) return false; // probe failed
-        z -= zprobe->getProbeHeight(); // relative distance between the probe points
+        z= zprobe->getProbeHeight() - z; // relative distance between the probe points, lower is negative z
         stream->printf("DEBUG: P%d:%1.4f\n", i, z);
         v[i].set(x, y, z);
     }
 
-    // if first point is not within tolerance of probe height report it.
-    if(abs(v[0][2] - zprobe->getProbeHeight()) > this->tolerance) {
+    // if first point is not within tolerance report it, it should ideally be 0
+    if(abs(v[0][2]) > this->tolerance) {
         stream->printf("WARNING: probe is not within tolerance\n");
     }
 
@@ -193,6 +218,7 @@ bool ThreePointStrategy::doProbing(StreamOutput *stream)
     if((mm.second - mm.first) <= this->tolerance) {
         this->plane= nullptr; // plane is flat no need to do anything
         stream->printf("DEBUG: flat plane\n");
+        // clear the adjustZfnc in robot
         THEKERNEL->robot->adjustZfnc= nullptr;
 
     }else{
@@ -223,3 +249,18 @@ std::tuple<float, float> ThreePointStrategy::parseXY(const char *str)
     }
     return std::make_tuple(x, y);
 }
+
+// parse a "X,Y,Z" string return x,y,z tuple
+std::tuple<float, float, float> ThreePointStrategy::parseXYZ(const char *str)
+{
+    float x = 0, y = 0, z= 0;
+    char *p;
+    x = strtof(str, &p);
+    if(p + 1 < str + strlen(str)) {
+        y = strtof(p + 1, &p);
+        if(p + 1 < str + strlen(str)) {
+            z = strtof(p + 1, nullptr);
+        }
+    }
+    return std::make_tuple(x, y, z);
+}

diff --git a/src/modules/tools/zprobe/ThreePointStrategy.h b/src/modules/tools/zprobe/ThreePointStrategy.h
index fe5de54..571bb43 100644 (file)
--- a/src/modules/tools/zprobe/ThreePointStrategy.h
+++ b/src/modules/tools/zprobe/ThreePointStrategy.h
@@ -24,7 +24,9 @@ private:
     void homeXY();
     bool doProbing(StreamOutput *stream);
     std::tuple<float, float> parseXY(const char *str);
+    std::tuple<float, float, float> parseXYZ(const char *str);
 
+    std::tuple<float, float, float> probe_offsets;
     std::tuple<float, float> probe_points[3];
     Plane3D *plane;
     struct {