com.willwinder.universalgcodesender.gcode.GcodePreprocessorUtils Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of ugs-core Show documentation
Universal Gcode Sender Library
The newest version!
/*
    Copyright 2013-2018 Will Winder

    This file is part of Universal Gcode Sender (UGS).

    UGS is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    UGS is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with UGS.  If not, see .
 */
package com.willwinder.universalgcodesender.gcode;

import com.willwinder.universalgcodesender.gcode.util.Code;
import static com.willwinder.universalgcodesender.gcode.util.Code.*;
import static com.willwinder.universalgcodesender.gcode.util.Code.ModalGroup.Motion;
import com.willwinder.universalgcodesender.gcode.util.PlaneFormatter;
import com.willwinder.universalgcodesender.i18n.Localization;
import com.willwinder.universalgcodesender.model.Position;
import java.text.DecimalFormat;
import java.util.ArrayList;
import java.util.Collections;
import java.util.LinkedHashSet;
import java.util.List;
import java.util.Objects;
import java.util.Set;
import java.util.regex.Matcher;
import java.util.regex.Pattern;
import java.util.stream.Collectors;

/**
 * Collection of useful command preprocessor methods.
 *
 * @author wwinder
 */
public class GcodePreprocessorUtils {

    private static final String EMPTY = "";
    public static final Pattern COMMENT = Pattern.compile("\\(.*\\)|\\s*;.*|%$");
    private static final Pattern COMMENTPARSE = Pattern.compile("(?<=\\()[^\\(\\)]*|(?<=\\;).*|%");
    private static final Pattern GCODE_PATTERN = Pattern.compile("[Gg]0*(\\d+)");

    private static int decimalLength = -1;
    private static Pattern decimalPattern;
    private static DecimalFormat decimalFormatter;

    /**
     * Searches the command string for an 'f' and replaces the speed value 
     * between the 'f' and the next space with a percentage of that speed.
     * In that way all speed values become a ratio of the provided speed 
     * and don't get overridden with just a fixed speed.
     */
    static public String overrideSpeed(String command, double speed) {
        String returnString = command;
        
        // Check if command sets feed speed.
        Pattern pattern = Pattern.compile("F([0-9.]+)", Pattern.CASE_INSENSITIVE);
        Matcher matcher = pattern.matcher(command);
        if (matcher.find()){
            Double originalFeedRate = Double.parseDouble(matcher.group(1));
            //System.out.println( "Found feed     " + originalFeedRate.toString() );
            Double newFeedRate      = originalFeedRate * speed / 100.0;
            //System.out.println( "Change to feed " + newFeedRate.toString() );
            returnString = matcher.replaceAll( "F" + newFeedRate.toString() );
        }

        return returnString;
    }
    
    /**
     * Removes any comments within parentheses or beginning with a semi-colon.
     */
    static public String removeComment(String command) {
        return COMMENT.matcher(command).replaceAll(EMPTY);
    }
    
    /**
     * Searches for a comment in the input string and returns the first match.
     */
    static public String parseComment(String command) {
        String comment = EMPTY;

        // REGEX: Find any comment, includes the comment characters:
        //              "(?<=\()[^\(\)]*|(?<=\;)[^;]*"
        //              "(?<=\\()[^\\(\\)]*|(?<=\\;)[^;]*"
        Matcher matcher = COMMENTPARSE.matcher(command);
        if (matcher.find()){
            comment = matcher.group(0);
        }

        return comment;
    }
    
    static public String truncateDecimals(int length, String command) {
        if (length != decimalLength) {
            //Only build the decimal formatter if the truncation length has changed.
            updateDecimalFormatter(length);

        }
        Matcher matcher = decimalPattern.matcher(command);

        // Build up the truncated command.
        Double d;
        StringBuffer sb = new StringBuffer();
        while (matcher.find()) {
            d = Double.parseDouble(matcher.group());
            matcher.appendReplacement(sb, decimalFormatter.format(d));
        }
        matcher.appendTail(sb);
        
        // Return new command.
        return sb.toString();
    }

    private static void updateDecimalFormatter(int length) {
        StringBuilder df = new StringBuilder();

        // Build up the decimal formatter.
        df.append("#");

        if (length != 0) {
            df.append(".");
        }
        for (int i = 0; i < length; i++) {
            df.append('#');
        }

        decimalFormatter = new DecimalFormat(df.toString(), Localization.dfs);

        // Build up the regular expression.
        df = new StringBuilder();
        df.append("\\d+\\.\\d");
        for (int i = 0; i < length; i++) {
            df.append("\\d");
        }
        df.append('+');
        decimalPattern = Pattern.compile(df.toString());
        decimalLength = length;
    }

    static public List parseCodes(List args, char code) {
        List l = new ArrayList<>();
        char address = Character.toUpperCase(code);
        
        for (String s : args) {
            if (s.length() > 0 && Character.toUpperCase(s.charAt(0)) == address) {
                l.add(s.substring(1));
            }
        }
        
        return l;
    }
    

    static public List parseGCodes(String command) {
        Matcher matcher = GCODE_PATTERN.matcher(command);
        List codes = new ArrayList<>();
        
        while (matcher.find()) {
            codes.add(Integer.parseInt(matcher.group(1)));
        }
        
        return codes;
    }

    static private Pattern mPattern = Pattern.compile("[Mm]0*(\\d+)");
    static public List parseMCodes(String command) {
        Matcher matcher = GCODE_PATTERN.matcher(command);
        List codes = new ArrayList<>();
        
        while (matcher.find()) {
            codes.add(Integer.parseInt(matcher.group(1)));
        }
        
        return codes;
    }

    /**
     * Update a point given the arguments of a command, using a pre-parsed list.
     */
    static public Position updatePointWithCommand(List commandArgs, Position initial, boolean absoluteMode) {

        Double x = parseCoord(commandArgs, 'X');
        Double y = parseCoord(commandArgs, 'Y');
        Double z = parseCoord(commandArgs, 'Z');

        if (x.isNaN() && y.isNaN() && z.isNaN()) {
            return null;
        }

        return updatePointWithCommand(initial, x, y, z, absoluteMode);
    }

    /**
     * Update a point given the new coordinates.
     */
    static public Position updatePointWithCommand(Position initial, double x, double y, double z, boolean absoluteMode) {

        Position newPoint = new Position(initial);

        if (absoluteMode) {
            if (!Double.isNaN(x)) {
                newPoint.x = x;
            }
            if (!Double.isNaN(y)) {
                newPoint.y = y;
            }
            if (!Double.isNaN(z)) {
                newPoint.z = z;
            }
        } else {
            if (!Double.isNaN(x)) {
                newPoint.x += x;
            }
            if (!Double.isNaN(y)) {
                newPoint.y += y;
            }
            if (!Double.isNaN(z)) {
                newPoint.z += z;
            }
        }

        return newPoint;
    }
    
    static public Position updateCenterWithCommand(
            List commandArgs,
            Position initial,
            Position nextPoint,
            boolean absoluteIJKMode,
            boolean clockwise,
            PlaneFormatter plane) {
        double i      = parseCoord(commandArgs, 'I');
        double j      = parseCoord(commandArgs, 'J');
        double k      = parseCoord(commandArgs, 'K');
        double radius = parseCoord(commandArgs, 'R');
        
        if (Double.isNaN(i) && Double.isNaN(j) && Double.isNaN(k)) {
            return GcodePreprocessorUtils.convertRToCenter(
                            initial, nextPoint, radius, absoluteIJKMode,
                            clockwise, plane);
        }

        return updatePointWithCommand(initial, i, j, k, absoluteIJKMode);

    }

    static public String generateLineFromPoints(final Code command, final Position start, final Position end, final boolean absoluteMode, DecimalFormat formatter) {
        DecimalFormat df = formatter;
        if (df == null) {
            df = new DecimalFormat("#.####");
        }
        
        StringBuilder sb = new StringBuilder();
        sb.append(command);

        if (absoluteMode) {
            if (!Double.isNaN(end.x)) {
                sb.append("X");
                sb.append(df.format(end.x));
            }
            if (!Double.isNaN(end.y)) {
                sb.append("Y");
                sb.append(df.format(end.y));
            }
            if (!Double.isNaN(end.z)) {
                sb.append("Z");
                sb.append(df.format(end.z));
            }
        } else { // calculate offsets.
            if (!Double.isNaN(end.x)) {
                sb.append("X");
                sb.append(df.format(end.x-start.x));
            }
            if (!Double.isNaN(end.y)) {
                sb.append("Y");
                sb.append(df.format(end.y-start.x));
            }
            if (!Double.isNaN(end.z)) {
                sb.append("Z");
                sb.append(df.format(end.z-start.x));
            }
        }
        
        return sb.toString();
    }
    
    /**
     * Splits a gcode command by each word/argument, doesn't care about spaces.
     * This command is about the same speed as the string.split(" ") command,
     * but might be a little faster using precompiled regex.
     */
    static public List splitCommand(String command) {
        // Special handling for GRBL system commands which will not be splitted
        if(command.startsWith("$")) {
            return Collections.singletonList(command);
        }

        List l = new ArrayList<>();
        boolean readNumeric = false;
        StringBuilder sb = new StringBuilder();
        
        for (int i = 0; i < command.length(); i++){
            char c = command.charAt(i);
            if (Character.isWhitespace(c)) continue;
                        
            // If the last character was numeric (readNumeric is true) and this
            // character is a letter or whitespace, then we hit a boundary.
            if (readNumeric && !Character.isDigit(c) && c != '.') {
                readNumeric = false; // reset flag.
                
                l.add(sb.toString());
                sb = new StringBuilder();
                
                if (Character.isLetter(c)) {
                    sb.append(c);
                }
            }

            else if (Character.isDigit(c) || c == '.' || c == '-') {
                sb.append(c);
                readNumeric = true;
            }
            
            else if (Character.isLetter(c)) {
                sb.append(c);
            }
        }
        
        // Add final one
        if (sb.length() > 0) {
            l.add(sb.toString());
        }
        
        return l;
    }
    
    // TODO: Replace everything that uses this with a loop that loops through
    //       the string and creates a hash with all the values.
    static public boolean hasAxisWords(List argList) {
        for(String t : argList) {
            if (t.length() > 1) {
                char c = Character.toUpperCase(t.charAt(0));
                if (c == 'X' || c == 'Y' || c == 'Z') {
                    return true;
                }
            }
        }
        return false;
    }

    // TODO: Replace everything that uses this with a loop that loops through
    //       the string and creates a hash with all the values.
    /**
     * Pulls out a word, like "F100", "S1300", "T0", "X-0.5"
     */
    static public String extractWord(List argList, char c) {
        char address = Character.toUpperCase(c);
        for(String t : argList)
        {
            if (Character.toUpperCase(t.charAt(0)) == address)
            {
                return t;
            }
        }
        return null;
    }

    // TODO: Replace everything that uses this with a loop that loops through
    //       the string and creates a hash with all the values.
    static public double parseCoord(List argList, char c)
    {
        String word = extractWord(argList, c);
        if (word != null && word.length() > 1) {
            try {
                return Double.parseDouble(word.substring(1));
            } catch (NumberFormatException e) {
                return Double.NaN;
            }
        }
        return Double.NaN;
    }
    
    /**
     * Generates the points along an arc including the start and end points.
     */
    static public List generatePointsAlongArcBDring(
            final Position start,
            final Position end,
            final Position center,
            boolean clockwise,
            double R,
            double minArcLength,
            double arcSegmentLength,
            PlaneFormatter plane) {
        double radius = R;

        // Calculate radius if necessary.
        if (radius == 0) {
            radius = Math.sqrt(Math.pow(plane.axis0(start) - plane.axis0(center),2.0) + Math.pow(plane.axis1(end) - plane.axis1(center), 2.0));
        }

        double startAngle = GcodePreprocessorUtils.getAngle(center, start, plane);
        double endAngle = GcodePreprocessorUtils.getAngle(center, end, plane);
        double sweep = GcodePreprocessorUtils.calculateSweep(startAngle, endAngle, clockwise);

        // Convert units.
        double arcLength = sweep * radius;

        // If this arc doesn't meet the minimum threshold, don't expand.
        if (minArcLength > 0 && arcLength < minArcLength) {
            return null;
        }

        int numPoints = 20;

        if (arcSegmentLength <= 0 && minArcLength > 0) {
            arcSegmentLength = (sweep * radius) / minArcLength;
        }

        if (arcSegmentLength > 0) {
            numPoints = (int)Math.ceil(arcLength/arcSegmentLength);
        }

        return GcodePreprocessorUtils.generatePointsAlongArcBDring(start, end, center, clockwise, radius, startAngle, sweep, numPoints, plane);
    }

    /**
     * Generates the points along an arc including the start and end points.
     */
    static private List generatePointsAlongArcBDring(
            final Position p1,
            final Position p2,
            final Position center,
            boolean isCw,
            double radius, 
            double startAngle,
            double sweep,
            int numPoints,
            PlaneFormatter plane) {

        Position lineStart = new Position(p1);
        List segments = new ArrayList<>();
        double angle;

        // Calculate radius if necessary.
        if (radius == 0) {
            radius = Math.sqrt(Math.pow(plane.axis0(p1) - plane.axis1(center), 2.0) + Math.pow(plane.axis1(p1) - plane.axis1(center), 2.0));
        }

        double zIncrement = (plane.linear(p2) - plane.linear(p1)) / numPoints;
        for(int i=0; i= Math.PI * 2) {
                angle = angle - Math.PI * 2;
            }

            //lineStart.x = Math.cos(angle) * radius + center.x;
            plane.setAxis0(lineStart, Math.cos(angle) * radius + plane.axis0(center));
            //lineStart.y = Math.sin(angle) * radius + center.y;
            plane.setAxis1(lineStart, Math.sin(angle) * radius + plane.axis1(center));
            //lineStart.z += zIncrement;
            plane.setLinear(lineStart, plane.linear(lineStart) + zIncrement);
            
            segments.add(new Position(lineStart));
        }
        
        segments.add(new Position(p2));

        return segments;
    }

    /**
     * Helper method for to convert IJK syntax to center point.
     * @return the center of rotation between two points with IJK codes.
     */
    static private Position convertRToCenter(
            Position start,
            Position end,
            double radius,
            boolean absoluteIJK,
            boolean clockwise,
            PlaneFormatter plane) {
        double R = radius;
        Position center = new Position();
        
        // This math is copied from GRBL in gcode.c
        double x = plane.axis0(end) - plane.axis0(start);
        double y = plane.axis1(end) - plane.axis1(start);

        double h_x2_div_d = 4 * R*R - x*x - y*y;
        //if (h_x2_div_d < 0) { System.out.println("Error computing arc radius."); }
        h_x2_div_d = (-Math.sqrt(h_x2_div_d)) / Math.hypot(x, y);

        if (!clockwise) {
            h_x2_div_d = -h_x2_div_d;
        }

        // Special message from gcoder to software for which radius
        // should be used.
        if (R < 0) {
            h_x2_div_d = -h_x2_div_d;
            // TODO: Places that use this need to run ABS on radius.
            radius = -radius;
        }

        double offsetX = 0.5*(x-(y*h_x2_div_d));
        double offsetY = 0.5*(y+(x*h_x2_div_d));

        if (!absoluteIJK) {
            plane.setAxis0(center, plane.axis0(start) + offsetX);
            plane.setAxis1(center, plane.axis1(start) + offsetY);
        } else {
            plane.setAxis0(center, offsetX);
            plane.setAxis1(center, offsetY);
        }

        return center;
    }

    /** 
     * Helper method for arc calculation
     * @return angle in radians of a line going from start to end.
     */
    static private double getAngle(final Position start, final Position end, PlaneFormatter plane) {
        double deltaX = plane.axis0(end) - plane.axis0(start);
        double deltaY = plane.axis1(end) - plane.axis1(start);

        double angle = 0.0;

        if (deltaX != 0) { // prevent div by 0
            // it helps to know what quadrant you are in
            if (deltaX > 0 && deltaY >= 0) {  // 0 - 90
                angle = Math.atan(deltaY/deltaX);
            } else if (deltaX < 0 && deltaY >= 0) { // 90 to 180
                angle = Math.PI - Math.abs(Math.atan(deltaY/deltaX));
            } else if (deltaX < 0 && deltaY < 0) { // 180 - 270
                angle = Math.PI + Math.abs(Math.atan(deltaY/deltaX));
            } else if (deltaX > 0 && deltaY < 0) { // 270 - 360
                angle = Math.PI * 2 - Math.abs(Math.atan(deltaY/deltaX));
            }
        }
        else {
            // 90 deg
            if (deltaY > 0) {
                angle = Math.PI / 2.0;
            }
            // 270 deg
            else {
                angle = Math.PI * 3.0 / 2.0;
            }
        }
      
        return angle;
    }

    /**
     * Helper method for arc calculation to calculate sweep from two angles.
     * @return sweep in radians.
     */
    static private double calculateSweep(double startAngle, double endAngle, boolean isCw) {
        double sweep;

        // Full circle
        if (startAngle == endAngle) {
            sweep = (Math.PI * 2);
            // Arcs
        } else {
            // Account for full circles and end angles of 0/360
            if (endAngle == 0) {
                endAngle = Math.PI * 2;
            }
            // Calculate distance along arc.
            if (!isCw && endAngle < startAngle) {
                sweep = ((Math.PI * 2 - startAngle) + endAngle);
            } else if (isCw && endAngle > startAngle) {
                sweep = ((Math.PI * 2 - endAngle) + startAngle);
            } else {
                sweep = Math.abs(endAngle - startAngle);
            }
        }

        return sweep;
    }

    static public Set getGCodes(List args) {
        List gCodeStrings = parseCodes(args, 'G');
        return gCodeStrings.stream()
                .map(c -> 'G' + c)
                .map(Code::lookupCode)
                .filter(Objects::nonNull)
                .collect(Collectors.toCollection(LinkedHashSet::new));
    }


    public static class SplitCommand {
        public String extracted;
        public String remainder;
    }

    public static boolean isMotionWord(char character) {
        char c = Character.toUpperCase(character);
        return 
                c == 'X' || c == 'Y' || c == 'Z'
                || c == 'U' || c == 'V' || c == 'W'
                || c == 'I' || c == 'J' || c == 'K'
                || c == 'R';
    }

    /**
     * Return extracted motion words and remainder words.
     * If the code is G0 or G1 and G53 is found, it will also be extracted:
     * http://linuxcnc.org/docs/html/gcode/g-code.html#gcode:g53
     */
    public static SplitCommand extractMotion(Code code, String command) {
        List args = splitCommand(command);
        if (args.isEmpty()) return null;
        
        StringBuilder extracted = new StringBuilder();
        StringBuilder remainder = new StringBuilder();

        boolean includeG53 = code == G0 || code == G1;
        for (String arg : args) {
            char c = arg.charAt(0);
            Code lookup = Code.lookupCode(arg);
            if (lookup.getType() == Motion && lookup != code) return null;
            if (lookup == code || isMotionWord(c) || (includeG53 && lookup == G53)) {
                extracted.append(arg);
            } else {
                remainder.append(arg);
            }
        }

        if (extracted.length() == 0) return null;

        SplitCommand sc = new SplitCommand();
        sc.extracted= extracted.toString();
        sc.remainder = remainder.toString();

        return sc;
    }
}