Fixes for state handling and stopping problems

diff --git a/src/py/bbctrl/Cmd.py b/src/py/bbctrl/Cmd.py
index 438e1f27a1481be223c26a66019e205d6a858ebc..66d9cb8995216c4ca424605e9b596b759140013f 100644 (file)
--- 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 7ec97470251da4554926bafdb04bc65282c03b4f..42f0469184da29982083179f6e71e4a1f89596aa 100644 (file)
--- 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 dd37dd4a39109daab50e2e8c10b880b07738fc3e..cd5cd541a1eadd2fdcd2ef63e6ab206cd62e500b 100644 (file)
--- 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 e8950a0550e49ba245a6b711f9c06042430e24b0..36ef78aa37e59d714c26f722daf04a007f279fc0 100644 (file)
--- 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 9e9807445253e1338e60a1dbb337dc14934cdd57..443ad74026328ad211e5edac9f1c2b0fa9dacea9 100644 (file)
--- 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 bfa102d88242d2798bbb1d0d1d5572610d2ae47d..14b7bcd47238fb7d10e786312492ac8655677b26 100644 (file)
--- 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 25e221dc8b6620e63fadacb371cd6032431150cb..d1443bfb7efda7e405d432b470331a83bc00fbf7 100644 (file)
--- 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 0b4ec095241203ae600b320fcde3e160ca9ebaf7..f5fad5b136272b103849f57dc8066c30b952dd48 100644 (file)
--- 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')