Add FluidNC probe support

winder · Nov 21, 2022 · d111b55 · d111b55
1 parent d432d5e
commit d111b55
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 5 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
@@ -29,11 +29,7 @@
 import com.willwinder.universalgcodesender.listeners.ControllerStatus;
 import com.willwinder.universalgcodesender.listeners.ControllerStatusBuilder;
 import com.willwinder.universalgcodesender.listeners.MessageType;
-import com.willwinder.universalgcodesender.model.Axis;
-import com.willwinder.universalgcodesender.model.CommunicatorState;
-import com.willwinder.universalgcodesender.model.Overrides;
-import com.willwinder.universalgcodesender.model.PartialPosition;
-import com.willwinder.universalgcodesender.model.UnitUtils;
+import com.willwinder.universalgcodesender.model.*;
 import com.willwinder.universalgcodesender.services.MessageService;
 import com.willwinder.universalgcodesender.types.GcodeCommand;
 import com.willwinder.universalgcodesender.utils.IGcodeStreamReader;
@@ -752,6 +748,11 @@ public void rawResponseListener(String response) {
             ThreadHelper.invokeLater(this::initializeController);
         }
 
+        if (FluidNCUtils.isProbeMessage(response)) {
+            Position p = FluidNCUtils.parseProbePosition(response, getFirmwareSettings().getReportingUnits());
+            listeners.forEach(l -> l.probeCoordinates(p));
+        }
+
         if (FluidNCUtils.isMessageResponse(response)) {
             MessageType messageType = MessageType.INFO;
             if (controllerStatus.getState() == ControllerState.CONNECTING) {

diff --git a/ugs-core/src/com/willwinder/universalgcodesender/firmware/fluidnc/FluidNCUtils.java b/ugs-core/src/com/willwinder/universalgcodesender/firmware/fluidnc/FluidNCUtils.java
@@ -30,9 +30,12 @@ public class FluidNCUtils {
 
     private static final String MESSAGE_REGEX = "\\[MSG:.*]";
     private static final Pattern MESSAGE_PATTERN = Pattern.compile(MESSAGE_REGEX);
+    private static final String PROBE_REGEX = "\\[PRB:.*]";
+    private static final Pattern PROBE_PATTERN = Pattern.compile(PROBE_REGEX);
     private static final String WELCOME_REGEX = "(?<protocolvendor>.*)\\s(?<protocolversion>[0-9a-z.]*)\\s\\[((?<fncvariant>[a-zA-Z]*)?\\s(v(?<fncversion>[0-9.]*))?)+.*]";
     private static final Pattern WELCOME_PATTERN = Pattern.compile(WELCOME_REGEX, Pattern.CASE_INSENSITIVE);
     private static final Pattern MACHINE_PATTERN = Pattern.compile("(?<=MPos:)(-?\\d*\\.?\\d*),(-?\\d*\\.?\\d*),(-?\\d*\\.?\\d*)(?:,(-?\\d*\\.?\\d+))?(?:,(-?\\d*\\.?\\d+))?(?:,(-?\\d*\\.?\\d+))?");
+    private static final Pattern PROBE_POSITION_PATTERN = Pattern.compile("\\[PRB:(-?\\d*\\.\\d*),(-?\\d*\\.\\d*),(-?\\d*\\.\\d*)(?:,(-?\\d*\\.?\\d+))?(?:,(-?\\d*\\.?\\d+))?(?:,(-?\\d*\\.?\\d+))?:\\d?]");
     private static final Pattern WORK_PATTERN = Pattern.compile("(?<=WPos:)(-?\\d*\\.?\\d*),(-?\\d*\\.?\\d*),(-?\\d*\\.?\\d*)(?:,(-?\\d*\\.?\\d+))?(?:,(-?\\d*\\.?\\d+))?(?:,(-?\\d*\\.?\\d+))?");
     private static final Pattern WCO_PATTERN = Pattern.compile("(?<=WCO:)(-?\\d*\\.?\\d*),(-?\\d*\\.?\\d*),(-?\\d*\\.?\\d*)(?:,(-?\\d*\\.?\\d+))?(?:,(-?\\d*\\.?\\d+))?(?:,(-?\\d*\\.?\\d+))?");
 
@@ -48,6 +51,19 @@ static protected Optional<String> parseMessageResponse(final String response) {
         return Optional.of(response.substring(5, response.length() - 1));
     }
 
+    public static boolean isProbeMessage(String response) {
+        return PROBE_PATTERN.matcher(response).find();
+    }
+
+    static protected Position parseProbePosition(final String response, final UnitUtils.Units units) {
+        // Don't parse failed probe response.
+        if (response.endsWith(":0]")) {
+            return Position.INVALID;
+        }
+
+        return GrblUtils.getPositionFromStatusString(response, PROBE_POSITION_PATTERN, units);
+    }
+
     public static boolean isWelcomeResponse(String response) {
         return WELCOME_PATTERN.matcher(response).find();
     }

diff --git a/ugs-core/src/com/willwinder/universalgcodesender/model/Position.java b/ugs-core/src/com/willwinder/universalgcodesender/model/Position.java
@@ -26,6 +26,7 @@ This file is part of Universal Gcode Sender (UGS).
 public class Position extends CNCPoint {
 
     public static final Position ZERO = new Position(0, 0, 0, 0, 0, 0, Units.MM);
+    public static final Position INVALID = new Position(Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Units.MM);
 
     private final Units units;