Added FluidNC probing (winder#1963)

BulkSecurityGeneratorProjectV2 · Aug 15, 2022 · 77ec8c4 · 77ec8c4
1 parent 816e1f9
commit 77ec8c4
Showing 1 changed file with 29 additions and 0 deletions.
diff --git a/ugs-core/src/com/willwinder/universalgcodesender/firmware/fluidnc/FluidNCController.java b/ugs-core/src/com/willwinder/universalgcodesender/firmware/fluidnc/FluidNCController.java
@@ -53,9 +53,11 @@
 import java.util.logging.Level;
 import java.util.logging.Logger;
 
+import static com.willwinder.universalgcodesender.Utils.formatter;
 import static com.willwinder.universalgcodesender.firmware.fluidnc.FluidNCUtils.GRBL_COMPABILITY_VERSION;
 import static com.willwinder.universalgcodesender.model.CommunicatorState.COMM_CHECK;
 import static com.willwinder.universalgcodesender.model.UnitUtils.Units.MM;
+import static com.willwinder.universalgcodesender.model.UnitUtils.scaleUnits;
 import static com.willwinder.universalgcodesender.utils.ControllerUtils.sendAndWaitForCompletion;
 
 public class FluidNCController implements IController, CommunicatorListener {
@@ -217,12 +219,39 @@ public void jogMachineTo(PartialPosition position, double feedRate) throws Excep
 
     @Override
     public void probe(String axis, double feedRate, double distance, UnitUtils.Units units) throws Exception {
+        LOGGER.log(Level.INFO,
+                String.format("Probing. axis: %s, feedRate: %s, distance: %s, units: %s",
+                        axis, feedRate, distance, units));
 
+        String probePattern = "G38.2 %s%s F%s";
+        String probeCommand = String.format(probePattern, axis, formatter.format(distance), formatter.format(feedRate));
+
+        GcodeCommand state = createCommand(GcodeUtils.unitCommand(units) + " G91 G49");
+        state.setTemporaryParserModalChange(true);
+
+        GcodeCommand probe = createCommand(probeCommand);
+        probe.setTemporaryParserModalChange(true);
+
+        sendCommandImmediately(state);
+        sendCommandImmediately(probe);
+
+        restoreParserModalState();
     }
 
     @Override
     public void offsetTool(String axis, double offset, UnitUtils.Units units) throws Exception {
+        String offsetPattern = "G43.1 %s%s";
+        String offsetCommand = String.format(offsetPattern,
+                axis,
+                formatter.format(offset * scaleUnits(units, MM)));
+
+        GcodeCommand state = createCommand("G21 G90");
+        state.setTemporaryParserModalChange(true);
+
+        sendCommandImmediately(state);
+        sendCommandImmediately(createCommand(offsetCommand));
 
+        restoreParserModalState();
     }
 
     @Override