From 8b5eac7ec58ed77067e21e65eb7b4aecb2002939 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Thu, 8 Mar 2018 15:39:12 -0800 Subject: [PATCH] Fixes for state handling and stopping problems --- src/py/bbctrl/Cmd.py | 18 ++++++++++--- src/py/bbctrl/Comm.py | 5 ++-- src/py/bbctrl/CommandQueue.py | 14 ++++++++++ src/py/bbctrl/Jog.py | 26 +++++++++---------- src/py/bbctrl/Mach.py | 49 +++++++++++++++++++++++------------ src/py/bbctrl/Planner.py | 1 + src/py/inevent/EventState.py | 4 +-- src/py/inevent/JogHandler.py | 9 +++++-- 8 files changed, 86 insertions(+), 40 deletions(-) diff --git a/src/py/bbctrl/Cmd.py b/src/py/bbctrl/Cmd.py index 438e1f2..66d9cb8 100644 --- a/src/py/bbctrl/Cmd.py +++ b/src/py/bbctrl/Cmd.py @@ -135,9 +135,9 @@ def dwell(seconds): return DWELL + encode_float(seconds) def pause(type): - if type == 'program': type = 2 - elif type == 'optional': type = 3 - elif type == 'pallet-change': type = 2 + if type == 'program': type = 1 + elif type == 'optional': type = 2 + elif type == 'pallet-change': type = 1 else: raise Exception('Unknown pause type "%s"' % type) return '%s%d' % (PAUSE, type) @@ -187,6 +187,18 @@ def decode_command(cmd): elif value.find('.') == -1: data['value'] = int(value) else: data['value'] = float(value) + elif cmd[0] == JOG: + data['type'] = 'jog' + + cmd = cmd[1:] + while len(cmd): + name = cmd[0] + value = decode_float(cmd[1:7]) + cmd = cmd[7:] + + if name in 'xyzabcuvw': data[name] = value + + elif cmd[0] == SEEK: data['type'] = 'seek' diff --git a/src/py/bbctrl/Comm.py b/src/py/bbctrl/Comm.py index 7ec9747..42f0469 100644 --- a/src/py/bbctrl/Comm.py +++ b/src/py/bbctrl/Comm.py @@ -222,9 +222,8 @@ class Comm(object): self.i2c_command(Cmd.CLEAR) - def pause(self, optional = False): - data = ord('1' if optional else '0') - self.i2c_command(Cmd.PAUSE, byte = data) + def pause(self): + self.i2c_command(Cmd.PAUSE, byte = ord('0')) # User pause def reboot(self): self.queue_command(Cmd.REBOOT) diff --git a/src/py/bbctrl/CommandQueue.py b/src/py/bbctrl/CommandQueue.py index dd37dd4..cd5cd54 100644 --- a/src/py/bbctrl/CommandQueue.py +++ b/src/py/bbctrl/CommandQueue.py @@ -34,6 +34,7 @@ log.setLevel(logging.WARNING) class CommandQueue(): def __init__(self): + self.lastEnqueueID = 0 self.releaseID = 0 self.q = deque() @@ -42,12 +43,25 @@ class CommandQueue(): def clear(self): + self.lastEnqueueID = 0 self.releaseID = 0 self.q.clear() + def _flush_cb(self, cb, *args, **kwargs): + self.clear() + cb(*args, **kwargs) + + + def flush(self, cb, *args, **kwargs): + id = self.lastEnqueueID + 1 + self.enqueue(id, False, self._flush_cb, cb, args, kwargs) + return id + + def enqueue(self, id, immediate, cb, *args, **kwargs): log.info('add(#%d, %s) releaseID=%d', id, immediate, self.releaseID) + self.lastEnqueueID = id self.q.append((id, immediate, cb, args, kwargs)) self._release() diff --git a/src/py/bbctrl/Jog.py b/src/py/bbctrl/Jog.py index e8950a0..36ef78a 100644 --- a/src/py/bbctrl/Jog.py +++ b/src/py/bbctrl/Jog.py @@ -40,22 +40,22 @@ class Jog(inevent.JogHandler): config = { "Logitech Logitech RumblePad 2 USB": { "deadband": 0.1, - "axes": [ABS_X, ABS_Y, ABS_RZ, ABS_Z], - "dir": [1, -1, -1, 1], - "arrows": [ABS_HAT0X, ABS_HAT0Y], - "speed": [0x120, 0x121, 0x122, 0x123], - "lock": [0x124, 0x125], - }, + "axes": [ABS_X, ABS_Y, ABS_RZ, ABS_Z], + "dir": [1, -1, -1, 1], + "arrows": [ABS_HAT0X, ABS_HAT0Y], + "speed": [0x120, 0x121, 0x122, 0x123], + "lock": [0x124, 0x125], + }, "default": { "deadband": 0.1, - "axes": [ABS_X, ABS_Y, ABS_RY, ABS_RX], - "dir": [1, -1, -1, 1], - "arrows": [ABS_HAT0X, ABS_HAT0Y], - "speed": [0x133, 0x130, 0x131, 0x134], - "lock": [0x136, 0x137], - } + "axes": [ABS_X, ABS_Y, ABS_RY, ABS_RX], + "dir": [1, -1, -1, 1], + "arrows": [ABS_HAT0X, ABS_HAT0Y], + "speed": [0x133, 0x130, 0x131, 0x134], + "lock": [0x136, 0x137], } + } super().__init__(config) @@ -88,9 +88,9 @@ class Jog(inevent.JogHandler): def changed(self): + scale = 1.0 if self.speed == 1: scale = 1.0 / 128.0 if self.speed == 2: scale = 1.0 / 32.0 if self.speed == 3: scale = 1.0 / 4.0 - if self.speed == 4: scale = 1.0 self.v = [x * scale for x in self.axes] diff --git a/src/py/bbctrl/Mach.py b/src/py/bbctrl/Mach.py index 9e98074..443ad74 100644 --- a/src/py/bbctrl/Mach.py +++ b/src/py/bbctrl/Mach.py @@ -104,18 +104,38 @@ class Mach(Comm): def _update(self, update): - state = self._get_state() - # Handle EStop - if 'xx' in update and state == 'ESTOPPED': self.planner.reset() + if update.get('xx', '') == 'ESTOPPED': self.planner.reset() # Update cycle now, if it has changed self._update_cycle() - # Continue after seek hold - if (state == 'HOLDING' and self.planner.is_synchronizing() and - self.ctrl.state.get('pr', '') == 'Switch found'): - self.unpause() + if (('xx' in update or 'pr' in update) and + self.ctrl.state.get('xx', '') == 'HOLDING'): + # Continue after seek hold + if (self.ctrl.state.get('pr', '') == 'Switch found' and + self.planner.is_synchronizing()): + self.unpause() + + # Continue after stop hold + if self.ctrl.state.get('pr', '') == 'User stop': + self.planner.stop() + self.planner.update_position() + self.ctrl.state.set('line', 0) + self._unpause() + + + def _unpause(self): + pause_reason = self.ctrl.state.get('pr', '') + if pause_reason in ['User pause', 'Switch found']: + self.planner.restart() + + if pause_reason in ['User pause', 'User stop', 'Switch found']: + super().i2c_command(Cmd.FLUSH) + super().resume() + + + super().i2c_command(Cmd.UNPAUSE) @overrides(Comm) @@ -176,6 +196,7 @@ class Mach(Comm): if position is not None: self.mdi('G28.3 %c%f' % (axis, position)) + super().resume() else: self._begin_cycle('homing') @@ -238,8 +259,6 @@ class Mach(Comm): def stop(self): if self._get_cycle() == 'idle': self._begin_cycle('running') super().i2c_command(Cmd.STOP) - self.planner.stop() - self.ctrl.state.set('line', 0) def pause(self): super().pause() @@ -249,16 +268,12 @@ class Mach(Comm): if self._get_state() != 'HOLDING': return pause_reason = self.ctrl.state.get('pr', '') - if pause_reason in ['User paused', 'Switch found']: - self.planner.restart() - super().resume() - - super().i2c_command(Cmd.UNPAUSE) + if pause_reason in ['User pause', 'Program pause']: + self._unpause() - def optional_pause(self): - # TODO this could work better as a variable, i.e. $op=1 - if self._get_cycle() == 'running': super().pause(True) + def optional_pause(self, enable = True): + super().queue_command('$op=%d' % enable) def set_position(self, axis, position): diff --git a/src/py/bbctrl/Planner.py b/src/py/bbctrl/Planner.py index bfa102d..14b7bcd 100644 --- a/src/py/bbctrl/Planner.py +++ b/src/py/bbctrl/Planner.py @@ -249,6 +249,7 @@ class Planner(): try: self.planner.stop() self.cmdq.clear() + self.update_position() except Exception as e: log.exception(e) diff --git a/src/py/inevent/EventState.py b/src/py/inevent/EventState.py index 25e221d..d1443bf 100644 --- a/src/py/inevent/EventState.py +++ b/src/py/inevent/EventState.py @@ -66,8 +66,8 @@ class EventState: return ("({:6.3f}, {:6.3f}, {:6.3f}) ".format(*self.get_joystick3d()) + "({:6.3f}, {:6.3f}, {:6.3f}) ".format(*self.get_joystickR3d()) + "({:2.0f}, {:2.0f}) ".format(*self.get_hat()) + - "({:d}, {:d}) ".format(*self.get_mouse()) + - "({:d}, {:d})".format(*self.get_wheel())) + "({:0.2f}, {:0.2f}) ".format(*self.get_mouse()) + + "({:0.2f}, {:0.2f})".format(*self.get_wheel())) def get_joystick(self): diff --git a/src/py/inevent/JogHandler.py b/src/py/inevent/JogHandler.py index 0b4ec09..f5fad5b 100644 --- a/src/py/inevent/JogHandler.py +++ b/src/py/inevent/JogHandler.py @@ -35,7 +35,12 @@ log.setLevel(logging.INFO) def axes_to_string(axes): - return '({:6.3f}, {:6.3f}, {:6.3f})'.format(*axes) + s = '' + for axis in axes: + if s: s += ', ' + else: s = '(' + s += '{:6.3f}'.format(axis) + return s + ')' def event_to_string(event, state): @@ -64,7 +69,7 @@ class JogHandler: def changed(self): - log.debug(axes_to_string(self.axes) + ' x {:d}'.format(self.speed)) + log.info(axes_to_string(self.axes) + ' x {:d}'.format(self.speed)) def up(self): log.debug('up') -- 2.27.0