Fix bug where planner position may not sync after jog

diff --git a/CHANGELOG.md b/CHANGELOG.md
index d740a466c0da208c2655d4ba899d50db0ab90d00..6aac9280def43fea741b6a2ca069ac670b948451 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -3,6 +3,8 @@ Buildbotics CNC Controller Firmware Changelog
 
 ## v0.4.10
  - Fix demo password check
+ - Fix bug were fast clicks could cause jog commands to arrive out of order.
+ - Fix bug bug where planner position may not sync after jog.
 
 ## v0.4.9
  - Enforce 6A per motor channel peak current limit.

diff --git a/src/avr/src/state.c b/src/avr/src/state.c
index 5e1ae586464f17f82b1fe7f971f78da019cd707c..e94069cd0d1d3806098daf39dd9289243a979731 100644 (file)
--- a/src/avr/src/state.c
+++ b/src/avr/src/state.c
@@ -47,6 +47,7 @@ static struct {
   bool unpause_requested;
 
   state_t state;
+  uint16_t state_count;
   hold_reason_t hold_reason;
 } s = {
   .flushing = true, // Start out flushing
@@ -87,6 +88,7 @@ static void _set_state(state_t state) {
   if (s.state == state) return; // No change
   if (s.state == STATE_ESTOPPED) return; // Can't leave EStop state
   s.state = state;
+  s.state_count++;
 }
 
 
@@ -201,6 +203,7 @@ void state_callback() {
 
 // Var callbacks
 PGM_P get_state() {return state_get_pgmstr(state_get());}
+uint16_t get_state_count() {return s.state_count;}
 PGM_P get_hold_reason() {return state_get_hold_reason_pgmstr(s.hold_reason);}
 
 

diff --git a/src/avr/src/vars.def b/src/avr/src/vars.def
index cfac5c99d454ba3cbd3908903a6cd95551d649fe..2308df0f55ed69b3d33369da6c1d82983b11a9cd 100644 (file)
--- a/src/avr/src/vars.def
+++ b/src/avr/src/vars.def
@@ -140,6 +140,7 @@ VAR(hw_id,          hid, str,   0,      0, 1) // Hardware ID
 VAR(estop,           es, b8,    0,      1, 1) // Emergency stop
 VAR(estop_reason,    er, pstr,  0,      0, 1) // Emergency stop reason
 VAR(state,           xx, pstr,  0,      0, 1) // Machine state
+VAR(state_count,     xc, u16,   0,      0, 1) // Machine state change count
 VAR(hold_reason,     pr, pstr,  0,      0, 1) // Machine pause reason
 VAR(underrun,        un, u32,   0,      0, 1) // Stepper buffer underrun count
 VAR(dwell_time,      dt, f32,   0,      0, 1) // Dwell timer

diff --git a/src/py/bbctrl/Comm.py b/src/py/bbctrl/Comm.py
index 74cc4e8ee61fa9930134b5811f653c46975cd6b7..46c087a7818e51fc56cac282b6aae0c4b16774f6 100644 (file)
--- a/src/py/bbctrl/Comm.py
+++ b/src/py/bbctrl/Comm.py
@@ -77,6 +77,7 @@ class Comm(object):
         self.last_motor_flags = [0] * 4
 
         avr.set_handlers(self._read, self._write)
+        self._poll_cb(False)
 
 
     def comm_next(self): raise Exception('Not implemented')
@@ -107,6 +108,12 @@ class Comm(object):
         self.flush()
 
 
+    def _poll_cb(self, now = True):
+        # Checks periodically for new commands from planner via comm_next()
+        if now: self.flush()
+        self.ctrl.ioloop.call_later(1, self._poll_cb)
+
+
     def _write(self, write_cb):
         # Finish writing current command
         if self.command is not None:

diff --git a/src/py/bbctrl/Mach.py b/src/py/bbctrl/Mach.py
index 347ad8e88f4bcf2c4fb5aad9a6c54bfb8181243b..689ffd4a9d21e5bf5ec8134f743f3b57f31ade99 100644 (file)
--- a/src/py/bbctrl/Mach.py
+++ b/src/py/bbctrl/Mach.py
@@ -77,10 +77,8 @@ class Mach(Comm):
 
         self.planner = bbctrl.Planner(ctrl)
         self.unpausing = False
-        self.last_cycle = 'idle'
 
         ctrl.state.set('cycle', 'idle')
-        self._update_cb(False)
 
         ctrl.state.add_listener(self._update)
 
@@ -92,7 +90,7 @@ class Mach(Comm):
     def _is_holding(self): return self._get_state() == 'HOLDING'
     def _is_ready(self): return self._get_state() == 'READY'
     def _get_pause_reason(self): return self.ctrl.state.get('pr', '')
-    def _get_cycle(self): return self.ctrl.state.get('cycle')
+    def _get_cycle(self): return self.ctrl.state.get('cycle', 'idle')
 
 
     def _is_paused(self):
@@ -101,67 +99,57 @@ class Mach(Comm):
             'User pause', 'Program pause', 'Optional pause')
 
 
+    def _set_cycle(self, cycle): self.ctrl.state.set('cycle', cycle)
+
+
     def _begin_cycle(self, cycle):
         current = self._get_cycle()
         if current == cycle: return # No change
 
-        # TODO handle jogging during pause
-        # if current == 'idle' or (cycle == 'jogging' and self._is_paused()):
-        if current == 'idle':
-            self.ctrl.state.set('cycle', cycle)
-            self.last_cycle = current
-
-        else:
+        if current != 'idle':
             raise Exception('Cannot enter %s cycle while in %s cycle' %
                             (cycle, current))
 
-
-    def _update_cb(self, now = True):
-        if now:
-            self._update_cycle()
-            self.flush()
-
-        self.ctrl.ioloop.call_later(1, self._update_cb)
-
-
-    def _update_cycle(self):
-        if (self._get_cycle() != 'idle' and self._is_ready() and
-            not self.planner.is_busy() and not super().is_active()):
-            self.planner.update_position()
-            self.ctrl.state.set('cycle', 'idle')
+        # TODO handle jogging during pause
+        # if current == 'idle' or (cycle == 'jogging' and self._is_paused()):
+        self._set_cycle(cycle)
 
 
     def _update(self, update):
-        # Handle EStop
-        if update.get('xx', '') == 'ESTOPPED': self.planner.reset(False)
-
         # Detect motor faults
         for motor in range(4):
             key = '%ddf' % motor
             if key in update and update[key] & 0x1f:
                 self.mlog.error(motor_fault_error % motor)
 
-        # Update cycle now, if it has changed
-        self._update_cycle()
+        # Get state
+        state_changed = 'xc' in update
+        state = self._get_state()
+
+        # Handle EStop
+        if state_changed and state == 'ESTOPPED': self.planner.reset(False)
+
+        # Exit cycle if state changed to READY
+        if (state_changed and self._get_cycle() != 'idle' and
+            self._is_ready() and not self.planner.is_busy() and
+            not super().is_active()):
+            self.planner.position_change()
+            self._set_cycle('idle')
 
         # Unpause sync
-        if update.get('xx', 'HOLDING') != 'HOLDING': self.unpausing = False
+        if state_changed and state != 'HOLDING': self.unpausing = False
 
         # Entering HOLDING state
-        if update.get('xx', '') == 'HOLDING':
+        if state_changed and state == 'HOLDING':
             # Always flush queue after pause
             super().i2c_command(Cmd.FLUSH)
             super().resume()
 
-            # Return from jogging cycle
-            if self._get_cycle() == 'jogging':
-                self.planner.update_position()
-                self.ctrl.state.set('cycle', self.last_cycle)
-
         # Automatically unpause after seek or stop hold
+        # Must be after holding commands above
         op = self.ctrl.state.get('optional_pause', False)
         pr = self._get_pause_reason()
-        if (('xx' in update or 'pr' in update) and self._is_holding() and
+        if ((state_changed or 'pr' in update) and self._is_holding() and
             (pr in ('Switch found', 'User stop') or
              (pr == 'Optional pause' and not op))):
             self._unpause()
@@ -173,7 +161,6 @@ 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()
@@ -242,6 +229,7 @@ class Mach(Comm):
 
     def jog(self, axes):
         self._begin_cycle('jogging')
+        self.planner.position_change()
         super().queue_command(Cmd.jog(axes))
 
 

diff --git a/src/py/bbctrl/Planner.py b/src/py/bbctrl/Planner.py
index 2ccc897a3d7d1f8f5257d0aedbf264e3bde29b6a..fcb19af723cd350b14ba993adc89d8c0d7610135 100644 (file)
--- a/src/py/bbctrl/Planner.py
+++ b/src/py/bbctrl/Planner.py
@@ -59,6 +59,7 @@ class Planner():
         self.log = ctrl.log.get('Planner')
         self.cmdq = CommandQueue(ctrl)
         self.planner = None
+        self._position_dirty = False
 
         ctrl.state.add_listener(self._update)
 
@@ -68,9 +69,12 @@ class Planner():
 
     def is_busy(self): return self.is_running() or self.cmdq.is_active()
     def is_running(self): return self.planner.is_running()
+    def position_change(self): self._position_dirty = True
 
 
-    def update_position(self):
+    def _sync_position(self, force = False):
+        if not force and not self._position_dirty: return
+        self._position_dirty = False
         self.planner.set_position(self.ctrl.state.get_position())
 
 
@@ -302,7 +306,7 @@ class Planner():
         self.planner.set_resolver(self._get_var_cb)
         # TODO logger is global and will not work correctly in demo mode
         self.planner.set_logger(self._log_cb, 1, 'LinePlanner:3')
-        self.update_position()
+        self._position_dirty = True
         self.cmdq.clear()
         self.reset_times()
         self.ctrl.state.reset()
@@ -310,6 +314,7 @@ class Planner():
 
     def mdi(self, cmd, with_limits = True):
         self.log.info('MDI:' + cmd)
+        self._sync_position()
         self.planner.load_string(cmd, self.get_config(True, with_limits))
         self.reset_times()
 
@@ -317,6 +322,7 @@ class Planner():
     def load(self, path):
         path = self.ctrl.get_path('upload', path)
         self.log.info('GCode:' + path)
+        self._sync_position()
         self.planner.load(path, self.get_config(False, True))
         self.reset_times()
 
@@ -341,6 +347,7 @@ class Planner():
             self.cmdq.clear()
             self.cmdq.release(id)
             self._plan_time_restart()
+            self._sync_position(True)
             self.planner.restart(id, position)
 
         except: