From 4e0c199c7c8bd9126b530a4bb990ab95cdcfea11 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Thu, 27 Jun 2019 15:29:15 -0700 Subject: [PATCH] Fix bug where planner position may not sync after jog --- CHANGELOG.md | 2 ++ src/avr/src/state.c | 3 ++ src/avr/src/vars.def | 1 + src/py/bbctrl/Comm.py | 7 +++++ src/py/bbctrl/Mach.py | 64 ++++++++++++++++------------------------ src/py/bbctrl/Planner.py | 11 +++++-- 6 files changed, 48 insertions(+), 40 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d740a46..6aac928 100644 --- 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 5e1ae58..e94069c 100644 --- 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 cfac5c9..2308df0 100644 --- 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 74cc4e8..46c087a 100644 --- 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 347ad8e..689ffd4 100644 --- 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 2ccc897..fcb19af 100644 --- 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: -- 2.27.0