From 5b23434828c578a4a18e94cca6b5fe48e7b7a448 Mon Sep 17 00:00:00 2001
From: Joseph Coffland <joseph@cauldrondevelopment.com>
Date: Wed, 19 Dec 2018 00:33:49 -0800
Subject: [PATCH] Update planner position correctly after stop

---
 src/py/bbctrl/Mach.py       | 1 +
 src/py/bbctrl/Planner.py    | 9 ++++++---
 src/py/bbctrl/Preplanner.py | 2 +-
 3 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/src/py/bbctrl/Mach.py b/src/py/bbctrl/Mach.py
index 7a5e962..9870154 100644
--- a/src/py/bbctrl/Mach.py
+++ b/src/py/bbctrl/Mach.py
@@ -170,6 +170,7 @@ class Mach(Comm):
 
         if pause_reason == 'User stop':
             self.planner.stop()
+            self.planner.update_position()
             self.ctrl.state.set('line', 0)
 
         else: self.planner.restart()
diff --git a/src/py/bbctrl/Planner.py b/src/py/bbctrl/Planner.py
index fc6e7e9..220d612 100644
--- a/src/py/bbctrl/Planner.py
+++ b/src/py/bbctrl/Planner.py
@@ -70,7 +70,11 @@ class Planner():
     def is_running(self): return self.planner.is_running()
 
 
-    def get_config(self, mdi, with_limits, with_position = True):
+    def update_position(self):
+        self.planner.set_position(self.ctrl.state.get_position())
+
+
+    def get_config(self, mdi, with_limits):
         state = self.ctrl.state
         config = self.ctrl.config
 
@@ -84,8 +88,6 @@ class Planner():
             # TODO junction deviation & accel
             }
 
-        if with_position: cfg['position'] = state.get_position()
-
         if with_limits:
             minLimit = state.get_soft_limit_vector('tn', -math.inf)
             maxLimit = state.get_soft_limit_vector('tm', math.inf)
@@ -289,6 +291,7 @@ class Planner():
         self.planner = gplan.Planner()
         self.planner.set_resolver(self._get_var_cb)
         self.planner.set_logger(self._log_cb, 1, 'LinePlanner:3')
+        self.update_position()
         self.cmdq.clear()
         self.reset_times()
         self.ctrl.state.reset()
diff --git a/src/py/bbctrl/Preplanner.py b/src/py/bbctrl/Preplanner.py
index 25a60f8..b2aaafc 100644
--- a/src/py/bbctrl/Preplanner.py
+++ b/src/py/bbctrl/Preplanner.py
@@ -148,7 +148,7 @@ class Preplanner(object):
 
         # Copy state for thread
         state = self.ctrl.state.snapshot()
-        config = self.ctrl.mach.planner.get_config(False, False, False)
+        config = self.ctrl.mach.planner.get_config(False, False)
         del config['default-units']
 
         # Start planner thread
-- 
2.27.0