fix rotary delta FK to be mirrored like the IK

[clinton/Smoothieware.git] / src / modules / tools / zprobe / ZProbe.cpp

diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp
index d3d2454..14abf69 100644 (file)
--- a/src/modules/tools/zprobe/ZProbe.cpp
+++ b/src/modules/tools/zprobe/ZProbe.cpp
@@ -41,6 +41,7 @@
 #define return_feedrate_checksum CHECKSUM("return_feedrate")
 #define probe_height_checksum    CHECKSUM("probe_height")
 #define gamma_max_checksum       CHECKSUM("gamma_max")
+#define reverse_z_direction_checksum CHECKSUM("reverse_z")
 
 // from endstop section
 #define delta_homing_checksum    CHECKSUM("delta_homing")
@@ -134,6 +135,7 @@ void ZProbe::on_config_reload(void *argument)
     this->fast_feedrate = THEKERNEL->config->value(zprobe_checksum, fast_feedrate_checksum)->by_default(100)->as_number(); // feedrate in mm/sec
     this->return_feedrate = THEKERNEL->config->value(zprobe_checksum, return_feedrate_checksum)->by_default(0)->as_number(); // feedrate in mm/sec
     this->max_z         = THEKERNEL->config->value(gamma_max_checksum)->by_default(500)->as_number(); // maximum zprobe distance
+    this->reverse_z     = THEKERNEL->config->value(reverse_z_direction_checksum)->by_default(false)->as_bool(); // Z probe moves in reverse direction (upside down rdelta)
 }
 
 bool ZProbe::wait_for_probe(int& steps)
@@ -183,7 +185,7 @@ bool ZProbe::wait_for_probe(int& steps)
 
 // single probe with custom feedrate
 // returns boolean value indicating if probe was triggered
-bool ZProbe::run_probe_feed(int& steps, float feedrate, float max_dist)
+bool ZProbe::run_probe(int& steps, float feedrate, float max_dist, bool reverse)
 {
     // not a block move so disable the last tick setting
     for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
@@ -196,11 +198,13 @@ bool ZProbe::run_probe_feed(int& steps, float feedrate, float max_dist)
     float maxz= max_dist < 0 ? this->max_z*2 : max_dist;
 
     // move Z down
-    STEPPER[Z_AXIS]->move(true, maxz * Z_STEPS_PER_MM, 0); // always probes down, no more than 2*maxz
+    bool dir= !reverse_z;
+    if(reverse) dir= !dir;  // specified to move in opposite Z direction
+    STEPPER[Z_AXIS]->move(dir, maxz * Z_STEPS_PER_MM, 0); // probe in specified direction, no more than maxz
     if(this->is_delta || this->is_rdelta) {
         // for delta need to move all three actuators
-        STEPPER[X_AXIS]->move(true, maxz * STEPS_PER_MM(X_AXIS), 0);
-        STEPPER[Y_AXIS]->move(true, maxz * STEPS_PER_MM(Y_AXIS), 0);
+        STEPPER[X_AXIS]->move(dir, maxz * STEPS_PER_MM(X_AXIS), 0);
+        STEPPER[Y_AXIS]->move(dir, maxz * STEPS_PER_MM(Y_AXIS), 0);
     }
 
     // start acceleration processing
@@ -214,15 +218,7 @@ bool ZProbe::run_probe_feed(int& steps, float feedrate, float max_dist)
     return r;
 }
 
-// single probe with either fast or slow feedrate
-// returns boolean value indicating if probe was triggered
-bool ZProbe::run_probe(int& steps, bool fast)
-{
-    float feedrate = (fast ? this->fast_feedrate : this->slow_feedrate);
-    return run_probe_feed(steps, feedrate);
-}
-
-bool ZProbe::return_probe(int steps)
+bool ZProbe::return_probe(int steps, bool reverse)
 {
     // move probe back to where it was
 
@@ -236,6 +232,7 @@ bool ZProbe::return_probe(int steps)
 
     this->current_feedrate = fr * Z_STEPS_PER_MM; // feedrate in steps/sec
     bool dir= steps < 0;
+    if(reverse) dir= !dir;
     steps= abs(steps);
 
     bool delta= (this->is_delta || this->is_rdelta);
@@ -306,20 +303,18 @@ void ZProbe::on_gcode_received(void *argument)
 
             int steps;
             bool probe_result;
-            if(gcode->has_letter('F')) {
-                probe_result = run_probe_feed(steps, gcode->get_value('F') / 60);
-            } else {
-                probe_result = run_probe(steps);
-            }
+            bool reverse= (gcode->has_letter('R') && gcode->get_value('R') != 0); // specify to probe in reverse direction
+            float rate= gcode->has_letter('F') ? gcode->get_value('F') / 60 : this->slow_feedrate;
+            probe_result = run_probe(steps, rate, -1, reverse);
 
             if(probe_result) {
                 gcode->stream->printf("Z:%1.4f C:%d\n", zsteps_to_mm(steps), steps);
                 // move back to where it started, unless a Z is specified
-                if(gcode->has_letter('Z')) {
+                if(gcode->has_letter('Z') && !is_rdelta) {
                     // set Z to the specified value, and leave probe where it is
                     THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
                 } else {
-                    return_probe(steps);
+                    return_probe(steps, reverse);
                 }
             } else {
                 gcode->stream->printf("ZProbe not triggered\n");
@@ -488,16 +483,14 @@ void ZProbe::probe_XYZ(Gcode *gcode, int axis)
     gcode->stream->printf("[PRB:%1.3f,%1.3f,%1.3f:%d]\n", pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], probeok);
     THEKERNEL->robot->set_last_probe_position(std::make_tuple(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], probeok));
 
-    if(!probeok) {
-        if(gcode->subcode == 2) {
-            // issue error if probe was not triggered and subcode == 2
-            gcode->stream->printf("ALARM:Probe fail\n");
-            THEKERNEL->call_event(ON_HALT, nullptr);
+    if(!probeok && gcode->subcode == 2) {
+        // issue error if probe was not triggered and subcode == 2
+        gcode->stream->printf("ALARM:Probe fail\n");
+        THEKERNEL->call_event(ON_HALT, nullptr);
 
-        }else{
-            // if the probe stopped the move we need to correct the last_milestone as it did not reach where it thought
-            THEKERNEL->robot->reset_position_from_current_actuator_position();
-        }
+    }else if(probeok){
+        // if the probe stopped the move we need to correct the last_milestone as it did not reach where it thought
+        THEKERNEL->robot->reset_position_from_current_actuator_position();
     }
 }