ACU: more care when setting up go_to

socs/agents/acu/agent.py

@@ -987,13 +987,15 @@ def limit_func(target):
         return limit_func, limits
 
     @inlineCallbacks
-    def _go_to_axis(self, session, axis, target):
+    def _go_to_axis(self, session, axis, target,
+                    state_feedback=None):
         """Execute a movement, using "Preset" mode, on a specific axis.
 
         Args:
           session: session object variable of the parent operation.
           axis (str): one of 'Azimuth', 'Elevation', 'Boresight'.
           target (float): target position.
+          state_feedback (dict): place to record state (see notes).
 
         Returns:
           ok (bool): True if the motion completed successfully and
@@ -1007,6 +1009,13 @@ def _go_to_axis(self, session, axis, target):
           completes, even if the requested position has been achieved
           (i.e. no actual motion was needed).
 
+          The state_feedback may be used to pipeline the initial parts
+          of the movement, so two functions aren't trying to command
+          at the same time.  The ``state_feedback`` dict should be
+          passed in initialized with ``{'state': 'init'}``.  When
+          initial commanding is finished, this function will update it
+          to `state="wait"`, and then on completion to `state="done"`.
+
         """
         # Step time in event loop.
         TICK_TIME = 0.1
@@ -1054,6 +1063,10 @@ def _go_to_axis(self, session, axis, target):
         State = Enum(f'{axis}State',
                      ['INIT', 'WAIT_MOVING', 'WAIT_STILL', 'FAIL', 'DONE'])
 
+        if state_feedback is None:
+            state_feedback = {}
+        state_feedback['state'] = 'init'
+
         # If this axis is "ignore", skip it.
         for _axis, short_name in [
                 ('Azimuth', 'az'),
@@ -1062,6 +1075,7 @@ def _go_to_axis(self, session, axis, target):
         ]:
             if _axis == axis and short_name in self.ignore_axes:
                 self.log.warn('Ignoring requested motion on {axis}', axis=axis)
+                state_feedback['state'] = 'done'
                 yield dsleep(1)
                 return True, 'axis successfully ignored'
 
@@ -1227,6 +1241,7 @@ def get_history(t):
 
             elif state == State.WAIT_STILL:
                 # Once moving, watch for end of motion.
+                state_feedback['state'] = 'wait'
                 if not mode_ok:
                     self.log.error('Unexpected axis mode transition; exiting.')
                     state = State.FAIL
@@ -1263,6 +1278,8 @@ def get_history(t):
             msg = 'Move aborted!'
         else:
             msg = 'Irregularity during motion!'
+
+        state_feedback['state'] = 'done'
         return success, msg
 
     @inlineCallbacks
@@ -1286,6 +1303,10 @@ def _go_to_axes(self, session, el=None, az=None, third=None,
             axis).
 
         """
+        # Construct args for each _go_to_axis command... don't create
+        # the Deferred here, because we will want to clear_faults
+        # first (and the Deferred might start running before that
+        # completes).
         move_defs = []
         for axis_name, short_name, target in [
                 ('Azimuth', 'az', az),
@@ -1294,15 +1315,27 @@ def _go_to_axes(self, session, el=None, az=None, third=None,
         ]:
             if target is not None:
                 move_defs.append(
-                    (short_name, self._go_to_axis(session, axis_name, target)))
+                    (short_name, (session, axis_name, target)))
+
         if len(move_defs) is None:
             return True, 'No motion requested.'
 
         if clear_faults:
             yield self.acu_control.clear_faults()
             yield dsleep(1)
 
-        moves = yield DeferredList([d for n, d in move_defs])
+        # Start each move, waiting for each to pass the "init" state
+        # before beginning the next one.
+        moves = []
+        for name, args in move_defs:
+            fb = {'state': 'init'}
+            move_def = self._go_to_axis(*args, state_feedback=fb)
+            while fb['state'] == 'init':
+                yield dsleep(.1)
+            moves.append(move_def)
+
+        # Now wait for all to complete.
+        moves = yield DeferredList(moves)
         all_ok, msgs = True, []
         for _ok, result in moves:
             if _ok:
@@ -1315,7 +1348,8 @@ def _go_to_axes(self, session, el=None, az=None, third=None,
         if all_ok:
             msg = msgs[0]
         else:
-            msg = ' '.join([f'{n}: {msg}' for (n, d), msg in zip(move_defs, msgs)])
+            msg = ' '.join([f'{name}: {msg}'
+                            for (name, args), msg in zip(move_defs, msgs)])
         return all_ok, msg
 
     @ocs_agent.param('az', type=float)