+ // if the probe stopped the move we need to correct the last_milestone as it did not reach where it thought
+ // this also sets last_milestone to the machine coordinates it stopped at
+ THEROBOT->reset_position_from_current_actuator_position();
+ float pos[3];
+ THEROBOT->get_axis_position(pos, 3);
+
+ 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", THEKERNEL->robot->from_millimeters(pos[X_AXIS]), THEKERNEL->robot->from_millimeters(pos[Y_AXIS]), THEKERNEL->robot->from_millimeters(pos[Z_AXIS]), probeok);
+ THEROBOT->set_last_probe_position(std::make_tuple(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], probeok));
+
+ if(probeok == 0 && (gcode->subcode == 2 || gcode->subcode == 4)) {
+ // issue error if probe was not triggered and subcode is 2 or 4
+ gcode->stream->printf("ALARM: Probe fail\n");
+ THEKERNEL->call_event(ON_HALT, nullptr);
+ }