From 95a83c811b4bd1fa06db148dfc226f33e56a89ff Mon Sep 17 00:00:00 2001
From: sliptonic <shopinthewoods@gmail.com>
Date: Wed, 22 Dec 2021 18:09:52 -0600
Subject: [PATCH] fix feedrate update Fix machinestate to handle drill moves
 and tests move feed and rapid declarations to PathGeom

---
 src/Mod/Path/PathFeedRate.py              | 18 +++++++++++-------
 src/Mod/Path/PathMachineState.py          | 17 +++++++++++++++++
 src/Mod/Path/PathScripts/PathGeom.py      |  3 ++-
 src/Mod/Path/PathTests/TestPathHelpers.py |  8 ++++++++
 4 files changed, 38 insertions(+), 8 deletions(-)

diff --git a/src/Mod/Path/PathFeedRate.py b/src/Mod/Path/PathFeedRate.py
index bd33265aab..06962d60e1 100644
--- a/src/Mod/Path/PathFeedRate.py
+++ b/src/Mod/Path/PathFeedRate.py
@@ -25,6 +25,7 @@ import PathScripts.PathLog as PathLog
 import PathMachineState
 import PathScripts.PathGeom as PathGeom
 import Part
+from PathScripts.PathGeom import CmdMoveRapid, CmdMoveAll
 
 __title__ = "Feed Rate Helper Utility"
 __author__ = "sliptonic (Brad Collette)"
@@ -43,6 +44,12 @@ else:
 
 
 def setFeedRate(commandlist, ToolController):
+    """Set the appropriate feed rate for a list of Path commands using the information from a Tool Controler
+
+    Every motion command in the list will have a feed rate parameter added or overwritten based
+    on the information stored in the tool controller. If a motion is a plunge (vertical) motion, the
+    VertFeed value will be used, otherwise the HorizFeed value will be used instead."""
+
     def _isVertical(currentposition, command):
         x = (
             command.Parameters["X"]
@@ -64,25 +71,22 @@ def setFeedRate(commandlist, ToolController):
             return True
         return PathGeom.isVertical(Part.makeLine(currentposition, endpoint))
 
-    feedcommands = ["G01", "G1", "G2", "G3", "G02", "G03", "G81", "G82", "G83"]
-    rapidcommands = ["G0", "G00"]
-
     machine = PathMachineState.MachineState()
 
     for command in commandlist:
-        if command.Name not in feedcommands + rapidcommands:
+        if command.Name not in CmdMoveAll:
             continue
 
-        if _isVertical(FreeCAD.Vector(machine.X, machine.Y, machine.Z), command):
+        if _isVertical(machine.getPosition(), command):
             rate = (
                 ToolController.VertRapid.Value
-                if command.Name in rapidcommands
+                if command.Name in CmdMoveRapid
                 else ToolController.VertFeed.Value
             )
         else:
             rate = (
                 ToolController.HorizRapid.Value
-                if command.Name in rapidcommands
+                if command.Name in CmdMoveRapid
                 else ToolController.HorizFeed.Value
             )
 
diff --git a/src/Mod/Path/PathMachineState.py b/src/Mod/Path/PathMachineState.py
index e57ebb9476..2e64dc4b97 100644
--- a/src/Mod/Path/PathMachineState.py
+++ b/src/Mod/Path/PathMachineState.py
@@ -29,6 +29,7 @@ __contributors__ = ""
 import PathScripts.PathLog as PathLog
 import FreeCAD
 from dataclasses import dataclass, field
+from PathScripts.PathGeom import CmdMoveRapid, CmdMoveAll, CmdMoveDrill
 
 if True:
     PathLog.setLevel(PathLog.Level.DEBUG, PathLog.thisModule())
@@ -92,6 +93,13 @@ class MachineState:
             self.WCS = command.Name
             return not oldstate == self.getState()
 
+        if command.Name in CmdMoveDrill:
+            oldZ = self.Z
+            for p in command.Parameters:
+                self.__setattr__(p, command.Parameters[p])
+            self.__setattr__("Z", oldZ)
+            return not oldstate == self.getState()
+
         for p in command.Parameters:
             self.__setattr__(p, command.Parameters[p])
 
@@ -116,3 +124,12 @@ class MachineState:
         state['T'] = self.T
 
         return state
+
+    def getPosition(self):
+        """
+        Returns a vector of the current machine position
+        """
+
+        # This is technical debt.  The actual position may include a rotation
+        # component as well.  We should probably be returning a placement
+        return FreeCAD.Vector(self.X, self.Y, self.Z)
diff --git a/src/Mod/Path/PathScripts/PathGeom.py b/src/Mod/Path/PathScripts/PathGeom.py
index bf45ca4141..7c8ab9681e 100644
--- a/src/Mod/Path/PathScripts/PathGeom.py
+++ b/src/Mod/Path/PathScripts/PathGeom.py
@@ -87,8 +87,9 @@ CmdMoveRapid = ["G0", "G00"]
 CmdMoveStraight = ["G1", "G01"]
 CmdMoveCW = ["G2", "G02"]
 CmdMoveCCW = ["G3", "G03"]
+CmdMoveDrill = ["G81", "G82", "G83"]
 CmdMoveArc = CmdMoveCW + CmdMoveCCW
-CmdMove = CmdMoveStraight + CmdMoveArc
+CmdMove = CmdMoveStraight + CmdMoveArc + CmdMoveDrill
 CmdMoveAll = CmdMove + CmdMoveRapid
 
 
diff --git a/src/Mod/Path/PathTests/TestPathHelpers.py b/src/Mod/Path/PathTests/TestPathHelpers.py
index 6810c21f43..3ac42e0273 100644
--- a/src/Mod/Path/PathTests/TestPathHelpers.py
+++ b/src/Mod/Path/PathTests/TestPathHelpers.py
@@ -53,6 +53,7 @@ class TestPathHelpers(PathTestBase):
         tc.HorizFeed = 20
 
         resultlist = PathFeedRate.setFeedRate(self.commandlist, tc)
+        print(resultlist)
 
         self.assertTrue(resultlist[0].Parameters["F"] == 5)
         self.assertTrue(resultlist[1].Parameters["F"] == 10)
@@ -101,3 +102,10 @@ class TestPathHelpers(PathTestBase):
         result = machine.addCommand(Path.Command("G0 X30"))
         self.assertTrue(result)
 
+        # Test that Drilling moves are handled correctly
+        result = machine.addCommand(Path.Command("G81 X50 Y50 Z0"))
+        state = machine.getState()
+        self.assertTrue(state['X'] == 50 )
+        self.assertTrue(state['Y'] == 50 )
+        self.assertTrue(state['Z'] == 5 )
+