+uint32_t ZProbe::read_probe(uint32_t dummy)
+{
+ if(!probing || probe_detected) return 0;
+
+ // TODO add debounce/noise filter
+ if(this->pin.get()) {
+ probe_detected= true;
+ // now tell all the stepper_motors to stop
+ for(auto &a : THEKERNEL->robot->actuators) a->force_finish_move();
+ }
+ return 0;
+}
+
+// special way to probe in the X or Y or Z direction using planned moves, should work with any kinematics
+void ZProbe::probe_XYZ(Gcode *gcode, int axis)
+{
+ // enable the probe checking in the timer
+ probing= true;
+ probe_detected= false;
+ THEKERNEL->robot->disable_segmentation= true; // we must disable segmentation as this won't work with it enabled (beware on deltas probing in X or Y)
+
+ // get probe feedrate if specified
+ float rate = (gcode->has_letter('F')) ? gcode->get_value('F')*60 : this->slow_feedrate;
+
+ // do a regular move which will stop as soon as the probe is triggered, or the distance is reached
+ switch(axis) {
+ case X_AXIS: coordinated_move(gcode->get_value('X'), 0, 0, rate, true); break;
+ case Y_AXIS: coordinated_move(0, gcode->get_value('Y'), 0, rate, true); break;
+ case Z_AXIS: coordinated_move(0, 0, gcode->get_value('Z'), rate, true); break;
+ }
+
+ // coordinated_move returns when the move is finished
+
+ // disable probe checking
+ probing= false;
+ THEKERNEL->robot->disable_segmentation= false;
+
+ float pos[3];
+ {
+ // get the current position
+ ActuatorCoordinates current_position{
+ THEKERNEL->robot->actuators[X_AXIS]->get_current_position(),
+ THEKERNEL->robot->actuators[Y_AXIS]->get_current_position(),
+ THEKERNEL->robot->actuators[Z_AXIS]->get_current_position()
+ };
+
+ // get machine position from the actuator position using FK
+ THEKERNEL->robot->arm_solution->actuator_to_cartesian(current_position, pos);
+ }
+
+ uint8_t probeok= this->probe_detected ? 1 : 0;
+
+ // print results using the GRBL format
+ 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 && 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(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();
+ }
+}
+