refactor endstops to get the homing logic ot of the handle gcode method

make G38 probe in Z use same new method of probing as XY so it works with all kinematics

diff --git a/src/modules/tools/endstops/Endstops.cpp b/src/modules/tools/endstops/Endstops.cpp
index ee04db3..6aed55b 100644 (file)
--- a/src/modules/tools/endstops/Endstops.cpp
+++ b/src/modules/tools/endstops/Endstops.cpp
@@ -615,181 +615,186 @@ void Endstops::home(char axes_to_move)
     this->status = NOT_HOMING;
 }
 
-// Start homing sequences by response to GCode commands
-void Endstops::on_gcode_received(void *argument)
+void Endstops::process_home_command(Gcode* gcode)
 {
-    Gcode *gcode = static_cast<Gcode *>(argument);
-    if ( gcode->has_g && gcode->g == 28) {
-        if( (gcode->subcode == 0 && THEKERNEL->is_grbl_mode()) || (gcode->subcode == 2 && !THEKERNEL->is_grbl_mode()) ) {
-            // G28 in grbl mode or G28.2 in normal mode will do a rapid to the predefined position
-            // TODO spec says if XYZ specified move to them first then move to MCS of specifed axis
-            char buf[32];
-            snprintf(buf, sizeof(buf), "G53 G0 X%f Y%f", saved_position[X_AXIS], saved_position[Y_AXIS]); // must use machine coordinates in case G92 or WCS is in effect
-            struct SerialMessage message;
-            message.message = buf;
-            message.stream = &(StreamOutput::NullStream);
-            THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); // as it is a multi G code command
-            return;
-
-        } else if(THEKERNEL->is_grbl_mode() && gcode->subcode == 2) { // G28.2 in grbl mode forces homing (triggered by $H)
-            // fall through so it does homing cycle
-
-        } else if(gcode->subcode == 1) { // G28.1 set pre defined position
-            // saves current position in absolute machine coordinates
-            THEKERNEL->robot->get_axis_position(saved_position);
-            return;
-
-        } else if(gcode->subcode == 3) { // G28.3 is a smoothie special it sets manual homing
-            if(gcode->get_num_args() == 0) {
-                THEKERNEL->robot->reset_axis_position(0, 0, 0);
-            } else {
-                // do a manual homing based on given coordinates, no endstops required
-                if(gcode->has_letter('X')) THEKERNEL->robot->reset_axis_position(gcode->get_value('X'), X_AXIS);
-                if(gcode->has_letter('Y')) THEKERNEL->robot->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
-                if(gcode->has_letter('Z')) THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
-            }
-            return;
+    if( (gcode->subcode == 0 && THEKERNEL->is_grbl_mode()) || (gcode->subcode == 2 && !THEKERNEL->is_grbl_mode()) ) {
+        // G28 in grbl mode or G28.2 in normal mode will do a rapid to the predefined position
+        // TODO spec says if XYZ specified move to them first then move to MCS of specifed axis
+        char buf[32];
+        snprintf(buf, sizeof(buf), "G53 G0 X%f Y%f", saved_position[X_AXIS], saved_position[Y_AXIS]); // must use machine coordinates in case G92 or WCS is in effect
+        struct SerialMessage message;
+        message.message = buf;
+        message.stream = &(StreamOutput::NullStream);
+        THEKERNEL->call_event(ON_CONSOLE_LINE_RECEIVED, &message ); // as it is a multi G code command
+        return;
+
+    } else if(THEKERNEL->is_grbl_mode() && gcode->subcode == 2) { // G28.2 in grbl mode forces homing (triggered by $H)
+        // fall through so it does homing cycle
 
-       } else if(gcode->subcode == 4) { // G28.4 is a smoothie special it sets manual homing based on the actuator position (used for rotary delta)
+    } else if(gcode->subcode == 1) { // G28.1 set pre defined position
+        // saves current position in absolute machine coordinates
+        THEKERNEL->robot->get_axis_position(saved_position);
+        return;
+
+    } else if(gcode->subcode == 3) { // G28.3 is a smoothie special it sets manual homing
+        if(gcode->get_num_args() == 0) {
+            THEKERNEL->robot->reset_axis_position(0, 0, 0);
+        } else {
             // do a manual homing based on given coordinates, no endstops required
-            float a=NAN, b=NAN, c=NAN;
-            if(gcode->has_letter('A')) a=  gcode->get_value('A');
-            if(gcode->has_letter('B')) b=  gcode->get_value('B');
-            if(gcode->has_letter('C')) c=  gcode->get_value('C');
-            THEKERNEL->robot->reset_actuator_position(a, b, c);
-            return;
-
-        } else if(THEKERNEL->is_grbl_mode()) {
-            gcode->stream->printf("error:Unsupported command\n");
-            return;
+            if(gcode->has_letter('X')) THEKERNEL->robot->reset_axis_position(gcode->get_value('X'), X_AXIS);
+            if(gcode->has_letter('Y')) THEKERNEL->robot->reset_axis_position(gcode->get_value('Y'), Y_AXIS);
+            if(gcode->has_letter('Z')) THEKERNEL->robot->reset_axis_position(gcode->get_value('Z'), Z_AXIS);
         }
+        return;
 
-        // G28 is received, we have homing to do
-
-        // First wait for the queue to be empty
-        THEKERNEL->conveyor->wait_for_empty_queue();
+    } 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
+        float a = NAN, b = NAN, c = NAN;
+        if(gcode->has_letter('A')) a =  gcode->get_value('A');
+        if(gcode->has_letter('B')) b =  gcode->get_value('B');
+        if(gcode->has_letter('C')) c =  gcode->get_value('C');
+        THEKERNEL->robot->reset_actuator_position(a, b, c);
+        return;
 
-        // Do we move select axes or all of them
-        char axes_to_move = 0;
-        // only enable homing if the endstop is defined, deltas, scaras always home all axis
-        bool home_all = this->is_delta || this->is_rdelta || this->is_scara || !( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') );
+    } else if(THEKERNEL->is_grbl_mode()) {
+        gcode->stream->printf("error:Unsupported command\n");
+        return;
+    }
 
-        for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
-            if ( (home_all || gcode->has_letter(c + 'X')) && this->pins[c + (this->home_direction[c] ? 0 : 3)].connected() ) {
-                axes_to_move += ( 1 << c );
-            }
-        }
+    // G28 is received, we have homing to do
 
-        // save current actuator position so we can report how far we moved
-        ActuatorCoordinates start_pos{
-            THEKERNEL->robot->actuators[X_AXIS]->get_current_position(),
-            THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(),
-            THEKERNEL->robot->actuators[Z_AXIS]->get_current_position()
-        };
+    // First wait for the queue to be empty
+    THEKERNEL->conveyor->wait_for_empty_queue();
 
-        // Enable the motors
-        THEKERNEL->stepper->turn_enable_pins_on();
-
-        // do the actual homing
-        if(homing_order != 0) {
-            // if an order has been specified do it in the specified order
-            // homing order is 0b00ccbbaa where aa is 0,1,2 to specify the first axis, bb is the second and cc is the third
-            // eg 0b00100001 would be Y X Z, 0b00100100 would be X Y Z
-            for (uint8_t m = homing_order; m != 0; m >>= 2) {
-                int a = (1 << (m & 0x03)); // axis to move
-                if((a & axes_to_move) != 0) {
-                    home(a);
-                }
-                // check if on_halt (eg kill)
-                if(THEKERNEL->is_halted()) break;
-            }
+    // Do we move select axes or all of them
+    char axes_to_move = 0;
+    // only enable homing if the endstop is defined, deltas, scaras always home all axis
+    bool home_all = this->is_delta || this->is_rdelta || this->is_scara || !( gcode->has_letter('X') || gcode->has_letter('Y') || gcode->has_letter('Z') );
 
-        } else {
-            // they all home at the same time
-            home(axes_to_move);
+    for ( int c = X_AXIS; c <= Z_AXIS; c++ ) {
+        if ( (home_all || gcode->has_letter(c + 'X')) && this->pins[c + (this->home_direction[c] ? 0 : 3)].connected() ) {
+            axes_to_move += ( 1 << c );
         }
+    }
 
-        // check if on_halt (eg kill)
-        if(THEKERNEL->is_halted()) {
-            if(!THEKERNEL->is_grbl_mode()) {
-                THEKERNEL->streams->printf("Homing cycle aborted by kill\n");
+    // save current actuator position so we can report how far we moved
+    ActuatorCoordinates start_pos{
+        THEKERNEL->robot->actuators[X_AXIS]->get_current_position(),
+        THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(),
+        THEKERNEL->robot->actuators[Z_AXIS]->get_current_position()
+    };
+
+    // Enable the motors
+    THEKERNEL->stepper->turn_enable_pins_on();
+
+    // do the actual homing
+    if(homing_order != 0) {
+        // if an order has been specified do it in the specified order
+        // homing order is 0b00ccbbaa where aa is 0,1,2 to specify the first axis, bb is the second and cc is the third
+        // eg 0b00100001 would be Y X Z, 0b00100100 would be X Y Z
+        for (uint8_t m = homing_order; m != 0; m >>= 2) {
+            int a = (1 << (m & 0x03)); // axis to move
+            if((a & axes_to_move) != 0) {
+                home(a);
             }
-            return;
+            // check if on_halt (eg kill)
+            if(THEKERNEL->is_halted()) break;
         }
 
-        // set the last probe position to the actuator units moved during this home
-        THEKERNEL->robot->set_last_probe_position(
-            std::make_tuple(
-                start_pos[0]-THEKERNEL->robot->actuators[0]->get_current_position(),
-                start_pos[1]-THEKERNEL->robot->actuators[1]->get_current_position(),
-                start_pos[2]-THEKERNEL->robot->actuators[2]->get_current_position(),
-                0));
-
-        if(home_all) {
-            // Here's where we would have been if the endstops were perfectly trimmed
-            // NOTE on a rotary delta home_offset is actuator position in degrees when homed and
-            // home_offset is the theta offset for each actuator, so M206 is used to set theta offset for each actuator in degrees
-            float ideal_position[3] = {
-                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 {
+        // they all home at the same time
+        home(axes_to_move);
+    }
 
-            bool has_endstop_trim = this->is_delta || this->is_scara;
-            if (has_endstop_trim) {
-                ActuatorCoordinates ideal_actuator_position;
-                THEKERNEL->robot->arm_solution->cartesian_to_actuator(ideal_position, ideal_actuator_position);
+    // check if on_halt (eg kill)
+    if(THEKERNEL->is_halted()) {
+        if(!THEKERNEL->is_grbl_mode()) {
+            THEKERNEL->streams->printf("Homing cycle aborted by kill\n");
+        }
+        return;
+    }
 
-                // We are actually not at the ideal position, but a trim away
-                ActuatorCoordinates real_actuator_position = {
-                    ideal_actuator_position[X_AXIS] - this->trim_mm[X_AXIS],
-                    ideal_actuator_position[Y_AXIS] - this->trim_mm[Y_AXIS],
-                    ideal_actuator_position[Z_AXIS] - this->trim_mm[Z_AXIS]
-                };
+    // set the last probe position to the actuator units moved during this home
+    THEKERNEL->robot->set_last_probe_position(
+        std::make_tuple(
+            start_pos[0] - THEKERNEL->robot->actuators[0]->get_current_position(),
+            start_pos[1] - THEKERNEL->robot->actuators[1]->get_current_position(),
+            start_pos[2] - THEKERNEL->robot->actuators[2]->get_current_position(),
+            0));
+
+    if(home_all) {
+        // Here's where we would have been if the endstops were perfectly trimmed
+        // NOTE on a rotary delta home_offset is actuator position in degrees when homed and
+        // home_offset is the theta offset for each actuator, so M206 is used to set theta offset for each actuator in degrees
+        float ideal_position[3] = {
+            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]
+        };
 
-                float real_position[3];
-                THEKERNEL->robot->arm_solution->actuator_to_cartesian(real_actuator_position, real_position);
-                // Reset the actuator positions to correspond our real position
-                THEKERNEL->robot->reset_axis_position(real_position[0], real_position[1], real_position[2]);
+        bool has_endstop_trim = this->is_delta || this->is_scara;
+        if (has_endstop_trim) {
+            ActuatorCoordinates ideal_actuator_position;
+            THEKERNEL->robot->arm_solution->cartesian_to_actuator(ideal_position, ideal_actuator_position);
 
-            } else {
-                // without endstop trim, real_position == ideal_position
-                if(is_rdelta) {
-                    // with a rotary delta we set the actuators angle then use the FK to calculate the resulting cartesian coordinates
-                    THEKERNEL->robot->reset_actuator_position(ideal_position[0], ideal_position[1], ideal_position[2]);
-
-                }else{
-                    // Reset the actuator positions to correspond our real position
-                    THEKERNEL->robot->reset_axis_position(ideal_position[0], ideal_position[1], ideal_position[2]);
-                }
-            }
+            // We are actually not at the ideal position, but a trim away
+            ActuatorCoordinates real_actuator_position = {
+                ideal_actuator_position[X_AXIS] - this->trim_mm[X_AXIS],
+                ideal_actuator_position[Y_AXIS] - this->trim_mm[Y_AXIS],
+                ideal_actuator_position[Z_AXIS] - this->trim_mm[Z_AXIS]
+            };
+
+            float real_position[3];
+            THEKERNEL->robot->arm_solution->actuator_to_cartesian(real_actuator_position, real_position);
+            // Reset the actuator positions to correspond our real position
+            THEKERNEL->robot->reset_axis_position(real_position[0], real_position[1], real_position[2]);
 
         } 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);
-                }
+            // without endstop trim, real_position == ideal_position
+            if(is_rdelta) {
+                // with a rotary delta we set the actuators angle then use the FK to calculate the resulting cartesian coordinates
+                THEKERNEL->robot->reset_actuator_position(ideal_position[0], ideal_position[1], ideal_position[2]);
+
+            } else {
+                // Reset the actuator positions to correspond our real position
+                THEKERNEL->robot->reset_axis_position(ideal_position[0], ideal_position[1], ideal_position[2]);
             }
         }
 
-        // on some systems where 0,0 is bed center it is nice to have home goto 0,0 after homing
-        // default is off for cartesian on for deltas
-        if(!is_delta) {
-            // NOTE a rotary delta usually has optical or hall-effect endstops so it is safe to go past them a little bit
-            if(this->move_to_origin_after_home) move_to_origin(axes_to_move);
-            // if limit switches are enabled we must back off endstop after setting home
-            back_off_home(axes_to_move);
-
-        } else if(this->move_to_origin_after_home || this->limit_enable[X_AXIS]) {
-            // deltas are not left at 0,0 because of the trim settings, so move to 0,0 if requested, but we need to back off endstops first
-            // also need to back off endstops if limits are enabled
-            back_off_home(axes_to_move);
-            if(this->move_to_origin_after_home) move_to_origin(axes_to_move);
+    } 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);
+            }
         }
     }
 
-    if (gcode->has_m) {
+    // on some systems where 0,0 is bed center it is nice to have home goto 0,0 after homing
+    // default is off for cartesian on for deltas
+    if(!is_delta) {
+        // NOTE a rotary delta usually has optical or hall-effect endstops so it is safe to go past them a little bit
+        if(this->move_to_origin_after_home) move_to_origin(axes_to_move);
+        // if limit switches are enabled we must back off endstop after setting home
+        back_off_home(axes_to_move);
+
+    } else if(this->move_to_origin_after_home || this->limit_enable[X_AXIS]) {
+        // deltas are not left at 0,0 because of the trim settings, so move to 0,0 if requested, but we need to back off endstops first
+        // also need to back off endstops if limits are enabled
+        back_off_home(axes_to_move);
+        if(this->move_to_origin_after_home) move_to_origin(axes_to_move);
+    }
+}
+
+// Start homing sequences by response to GCode commands
+void Endstops::on_gcode_received(void *argument)
+{
+    Gcode *gcode = static_cast<Gcode *>(argument);
+    if ( gcode->has_g && gcode->g == 28) {
+        process_home_command(gcode);
+
+    }else if (gcode->has_m) {
+
         switch (gcode->m) {
             case 119: {
                 for (int i = 0; i < 6; ++i) {
@@ -808,7 +813,7 @@ void Endstops::on_gcode_received(void *argument)
                     if (gcode->has_letter('Z')) home_offset[2] = gcode->get_value('Z');
                     gcode->stream->printf("X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]);
 
-                }else{
+                } else {
                     // set theta offset
                     if (gcode->has_letter('A')) home_offset[0] = gcode->get_value('A');
                     if (gcode->has_letter('B')) home_offset[1] = gcode->get_value('B');
@@ -836,7 +841,7 @@ void Endstops::on_gcode_received(void *argument)
 
                     gcode->stream->printf("Homing Offset: X %5.3f Y %5.3f Z %5.3f\n", home_offset[0], home_offset[1], home_offset[2]);
 
-                }else{
+                } else {
                     // for a rotary delta M306 calibrates the homing angle
                     // by doing M306 A-56.17 it will calculate the M206 A value (the theta offset for actuator A) based on the difference
                     // between what it thinks is the current angle and what the current angle actually is specified by A (ditto for B and C)
@@ -850,18 +855,18 @@ void Endstops::on_gcode_received(void *argument)
 
                     //figure out what home_offset needs to be to correct the homing_position
                     if (gcode->has_letter('A')) {
-                        float a= gcode->get_value('A'); // what actual angle is
-                        home_offset[0]= (a - current_angle[0]);
+                        float a = gcode->get_value('A'); // what actual angle is
+                        home_offset[0] = (a - current_angle[0]);
                         THEKERNEL->robot->reset_actuator_position(a, NAN, NAN);
                     }
                     if (gcode->has_letter('B')) {
-                        float b= gcode->get_value('B');
-                        home_offset[1]= (b - current_angle[1]);
+                        float b = gcode->get_value('B');
+                        home_offset[1] = (b - current_angle[1]);
                         THEKERNEL->robot->reset_actuator_position(NAN, b, NAN);
                     }
                     if (gcode->has_letter('C')) {
-                        float c= gcode->get_value('C');
-                        home_offset[2]= (c - current_angle[2]);
+                        float c = gcode->get_value('C');
+                        home_offset[2] = (c - current_angle[2]);
                         THEKERNEL->robot->reset_actuator_position(NAN, NAN, c);
                     }
 
@@ -922,20 +927,20 @@ void Endstops::on_gcode_received(void *argument)
 
                     if (gcode->has_letter('X')) {
                         float v = gcode->get_value('X');
-                        if(gcode->subcode == 2) x= lroundf(v * STEPS_PER_MM(X_AXIS));
-                        else x= roundf(v);
+                        if(gcode->subcode == 2) x = lroundf(v * STEPS_PER_MM(X_AXIS));
+                        else x = roundf(v);
                         STEPPER[X_AXIS]->move(x < 0, abs(x), f);
                     }
                     if (gcode->has_letter('Y')) {
                         float v = gcode->get_value('Y');
-                        if(gcode->subcode == 2) y= lroundf(v * STEPS_PER_MM(Y_AXIS));
-                        else y= roundf(v);
+                        if(gcode->subcode == 2) y = lroundf(v * STEPS_PER_MM(Y_AXIS));
+                        else y = roundf(v);
                         STEPPER[Y_AXIS]->move(y < 0, abs(y), f);
                     }
                     if (gcode->has_letter('Z')) {
                         float v = gcode->get_value('Z');
-                        if(gcode->subcode == 2) z= lroundf(v * STEPS_PER_MM(Z_AXIS));
-                        else z= roundf(v);
+                        if(gcode->subcode == 2) z = lroundf(v * STEPS_PER_MM(Z_AXIS));
+                        else z = roundf(v);
                         STEPPER[Z_AXIS]->move(z < 0, abs(z), f);
                     }
                     gcode->stream->printf("Moving X %ld Y %ld Z %ld steps at F %ld steps/sec\n", x, y, z, f);
@@ -996,8 +1001,8 @@ void Endstops::on_get_public_data(void* argument)
         pdr->set_taken();
 
     } else if(pdr->second_element_is(get_homing_status_checksum)) {
-        bool *homing= static_cast<bool *>(pdr->get_data_ptr());
-        *homing= this->status != NOT_HOMING;
+        bool *homing = static_cast<bool *>(pdr->get_data_ptr());
+        *homing = this->status != NOT_HOMING;
         pdr->set_taken();
     }
 }

diff --git a/src/modules/tools/endstops/Endstops.h b/src/modules/tools/endstops/Endstops.h
index 0aae671..844d3b0 100644 (file)
--- a/src/modules/tools/endstops/Endstops.h
+++ b/src/modules/tools/endstops/Endstops.h
@@ -14,6 +14,7 @@
 #include <bitset>
 
 class StepperMotor;
+class Gcode;
 
 class Endstops : public Module{
     public:
@@ -36,6 +37,7 @@ class Endstops : public Module{
         void on_set_public_data(void* argument);
         void on_idle(void *argument);
         bool debounced_get(int pin);
+        void process_home_command(Gcode* gcode);
 
         float homing_position[3];
         float home_offset[3];

diff --git a/src/modules/tools/zprobe/ZProbe.cpp b/src/modules/tools/zprobe/ZProbe.cpp
index 4661a08..9a4b501 100644 (file)
--- a/src/modules/tools/zprobe/ZProbe.cpp
+++ b/src/modules/tools/zprobe/ZProbe.cpp
@@ -370,53 +370,18 @@ void ZProbe::on_gcode_received(void *argument)
 
         if(gcode->has_letter('X')) {
             // probe in the X axis
-            probe_XY(gcode, X_AXIS);
+            probe_XYZ(gcode, X_AXIS);
 
         }else if(gcode->has_letter('Y')) {
             // probe in the Y axis
-            probe_XY(gcode, Y_AXIS);
+            probe_XYZ(gcode, Y_AXIS);
 
         }else if(gcode->has_letter('Z')) {
-            // we need to know where we started the probe from
-            float current_machine_pos[3];
-            THEKERNEL->robot->get_axis_position(current_machine_pos);
-
-            // probe down in the Z axis no more than the Z value in mm
-            float rate = (gcode->has_letter('F')) ? gcode->get_value('F') / 60 : this->slow_feedrate;
-            int steps;
-            bool probe_result = run_probe_feed(steps, rate, gcode->get_value('Z'));
-
-            if(probe_result) {
-                float z= current_machine_pos[Z_AXIS] - zsteps_to_mm(steps);
-                THEKERNEL->robot->set_last_probe_position(std::make_tuple(current_machine_pos[X_AXIS], current_machine_pos[Y_AXIS], z, 1));
-
-                if(THEKERNEL->is_grbl_mode()) {
-                    gcode->stream->printf("[PRB:%1.3f,%1.3f,%1.3f:1]\n", current_machine_pos[X_AXIS], current_machine_pos[Y_AXIS], z);
-
-                }else{
-                    gcode->stream->printf("INFO: delta Z %1.4f (Steps %d)\n", steps / Z_STEPS_PER_MM, steps);
-                }
-
-                // set position to where it stopped
-                THEKERNEL->robot->reset_axis_position(z, Z_AXIS);
-
-            } else {
-                THEKERNEL->robot->set_last_probe_position(std::make_tuple(current_machine_pos[X_AXIS], current_machine_pos[Y_AXIS], current_machine_pos[Z_AXIS], 0));
-                if(THEKERNEL->is_grbl_mode()) {
-                    if(gcode->subcode == 2) {
-                        gcode->stream->printf("ALARM:Probe fail\n");
-                        THEKERNEL->call_event(ON_HALT, nullptr);
-                    }
-                    gcode->stream->printf("[PRB:%1.3f,%1.3f,%1.3f:0]\n", current_machine_pos[X_AXIS], current_machine_pos[Y_AXIS], current_machine_pos[Z_AXIS]);
-
-                }else{
-                    gcode->stream->printf("error:ZProbe not triggered\n");
-                }
-            }
+            // probe in the Z axis
+            probe_XYZ(gcode, Z_AXIS);
 
         }else{
             gcode->stream->printf("error:at least one of X Y or Z must be specified\n");
-
         }
 
         // restore compensationTransform
@@ -459,8 +424,8 @@ void ZProbe::on_gcode_received(void *argument)
     }
 }
 
-// special way to probe in the X or Y direction
-void ZProbe::probe_XY(Gcode *gcode, int axis)
+// special way to probe in the X or Y or Z direction using planned moves
+void ZProbe::probe_XYZ(Gcode *gcode, int axis)
 {
     // enable the probe checking in the stepticker
     THEKERNEL->step_ticker->probe_fnc= [this]() { return this->pin.get(); };
@@ -475,6 +440,9 @@ void ZProbe::probe_XY(Gcode *gcode, int axis)
     }else if(axis == Y_AXIS) {
         coordinated_move(0, gcode->get_value('Y'), 0, rate, true);
 
+    }else if(axis == Z_AXIS) {
+        coordinated_move(0, 0, gcode->get_value('Z'), rate, true);
+
     }else{
         // this is an error
         THEKERNEL->step_ticker->probe_fnc= nullptr;

diff --git a/src/modules/tools/zprobe/ZProbe.h b/src/modules/tools/zprobe/ZProbe.h
index f33f8c7..f0376a5 100644 (file)
--- a/src/modules/tools/zprobe/ZProbe.h
+++ b/src/modules/tools/zprobe/ZProbe.h
@@ -50,7 +50,7 @@ public:
 
 private:
     void accelerate(int c);
-    void probe_XY(Gcode *gc, int axis);
+    void probe_XYZ(Gcode *gc, int axis);
 
     volatile float current_feedrate;
     float slow_feedrate;

diff --git a/src/modules/utils/simpleshell/SimpleShell.cpp b/src/modules/utils/simpleshell/SimpleShell.cpp
index 5b434af..f2390a5 100644 (file)
--- a/src/modules/utils/simpleshell/SimpleShell.cpp
+++ b/src/modules/utils/simpleshell/SimpleShell.cpp
@@ -685,7 +685,6 @@ void SimpleShell::grblDP_command( string parameters, StreamOutput *stream)
     stream->printf("[PRB:%1.3f,%1.3f,%1.3f:%d]\n", px, py, pz, ps);
 }
 
-// used to test out the get public data events
 void SimpleShell::get_command( string parameters, StreamOutput *stream)
 {
     string what = shift_parameter( parameters );