Repository: simonsobs/socs

Issue:
ACU: Sun avoidance check crashes when move_to(here)?
The full logs are lost, unfortunately, but here's the error:

```
Traceback (most recent call last):
  File "<string>", line 29, in <module>
  File "/usr/local/lib/python3.10/dist-packages/sorunlib/acu.py", line 15, in move_to
    check_response(acu, resp)
  File "/usr/local/lib/python3.10/dist-packages/sorunlib/_internal.py", line 48, in check_response
    raise RuntimeError(error)
RuntimeError: Task go_to in Agent acu failed to complete " +             "successfully.
OCSReply: OK : Operation "go_to" is currently not running (FAILED).
  go_to[session=9]; status=done with ERROR 0.015818 s ago, took 1.0 s
  messages (4 of 4):
    1703031719.083 Status is now "starting".
    1703031720.091 Status is now "running".
    1703031720.106 CRASH: [Failure instance: Traceback: <class 'ValueError'>: need at least one array to concatenate
/so/home/mhasse/.local/lib/python3.10/site-packages/twisted/internet/defer.py:696:callback
/so/home/mhasse/.local/lib/python3.10/site-packages/twisted/internet/defer.py:798:_startRunCallbacks
/so/home/mhasse/.local/lib/python3.10/site-packages/twisted/internet/defer.py:892:_runCallbacks
/so/home/mhasse/.local/lib/python3.10/site-packages/twisted/internet/defer.py:1792:gotResult
--- <exception caught here> ---
/so/home/mhasse/.local/lib/python3.10/site-packages/twisted/internet/defer.py:1697:_inlineCallbacks
/so/home/mhasse/.local/lib/python3.10/site-packages/socs/agents/acu/agent.py:1303:go_to
/so/home/mhasse/.local/lib/python3.10/site-packages/socs/agents/acu/agent.py:2437:_get_sunsafe_moves
/so/home/mhasse/.local/lib/python3.10/site-packages/socs/agents/acu/avoidance.py:437:analyze_paths
/so/home/mhasse/.local/lib/python3.10/site-packages/socs/agents/acu/avoidance.py:687:get_traj
/so/home/mhasse/.local/lib/python3.10/site-packages/numpy/core/shape_base.py:359:hstack
]
```

Discussion:
mhasself: (Just added fixes to address #565 as well.)

Patch:
diff --git a/docs/agents/hwp_pcu.rst b/docs/agents/hwp_pcu.rst
index b1ff8b6..7b91695 100644
--- a/docs/agents/hwp_pcu.rst
+++ b/docs/agents/hwp_pcu.rst
@@ -6,10 +6,12 @@
 HWP PCU Agent
 =============
 
-The HWP Phase Compensation Unit Agent interfaces with a 8 channel USB relay module
+The HWP Phase Compensation Unit (PCU) Agent interfaces with a 8 channel USB relay module
 (Numato Lab, product Number SKU:RL80001) to apply the discrete phase compensation in
 120-degree increments for the HWP motor drive circuit. When used in conjunction with
 a HWP pid controller, phase compensation in 60-degree increments can be achieved.
+The HWP PCU can also force the HWP to stop by making the phases of all three-phase
+motors the same.
 
 .. argparse::
     :filename: ../socs/agents/hwp_pcu/agent.py
diff --git a/socs/agents/acu/agent.py b/socs/agents/acu/agent.py
index 4d81a1d..4eefb92 100644
--- a/socs/agents/acu/agent.py
+++ b/socs/agents/acu/agent.py
@@ -5,6 +5,7 @@ import time
 from enum import Enum
 
 import numpy as np
+import ocs
 import soaculib as aculib
 import soaculib.status_keys as status_keys
 import twisted.web.client as tclient
@@ -100,13 +101,18 @@ class ACUAgent:
         fov_radius (float): If set, override the default Sun
             avoidance radius (i.e. the radius of the field of view, in
             degrees, to use for Sun avoidance purposes).
+        named_positions (list of str): Names and target positions to
+            register for use with go_to_named task.  If provided, each
+            entry in the list should be of the form "name=az,el" with
+            az and el parseable as floats; e.g. "home=180,45.1".
 
     """
 
     def __init__(self, agent, acu_config='guess', exercise_plan=None,
                  startup=False, ignore_axes=None, disable_idle_reset=False,
                  min_el=None, max_el=None,
-                 avoid_sun=None, fov_radius=None):
+                 avoid_sun=None, fov_radius=None,
+                 named_positions=None):
         self.log = agent.log
 
         # Separate locks for exclusive access to az/el, and boresight motions.
@@ -154,6 +160,17 @@ class ACUAgent:
         self._reset_sun_params(enabled=avoid_sun,
                                radius=fov_radius)
 
+        self.named_positions = {}
+        if named_positions:
+            for text in named_positions:
+                try:
+                    name, target = text.split('=')
+                    _az, _el = target.split(',')
+                    self.named_positions[name] = (float(_az), float(_el))
+                except Exception as e:
+                    agent.log.error('Failed to parse named_position string: {text}', text=text)
+                    raise e
+
         self.exercise_plan = exercise_plan
 
         # self.data provides a place to reference data from the monitors.
@@ -270,6 +287,9 @@ class ACUAgent:
                             self.go_to,
                             blocking=False,
                             aborter=self._simple_task_abort)
+        agent.register_task('go_to_named',
+                            self.go_to_named,
+                            blocking=False)
         agent.register_task('set_scan_params',
                             self.set_scan_params,
                             blocking=False)
@@ -393,6 +413,12 @@ class ACUAgent:
             "StatusResponseRate": 19.237531827325963,
             "PlatformType": "satp",
             "IgnoredAxes": [],
+            "NamedPositions": {
+              "home": [
+                180,
+                40
+              ]
+            },
             "DefaultScanParams": {
               "az_speed": 2.0,
               "az_accel": 1.0,
@@ -416,6 +442,7 @@ class ACUAgent:
                         'DefaultScanParams': self.scan_params,
                         'StatusResponseRate': 0.,
                         'IgnoredAxes': self.ignore_axes,
+                        'NamedPositions': self.named_positions,
                         'connected': False}
         not_data_keys = list(session.data.keys())
 
@@ -1259,14 +1286,14 @@ class ACUAgent:
     @ocs_agent.param('end_stop', default=False, type=bool)
     @inlineCallbacks
     def go_to(self, session, params):
-        """go_to(az=None, el=None, end_stop=False)
+        """go_to(az, el, end_stop=False)
 
         **Task** - Move the telescope to a particular point (azimuth,
         elevation) in Preset mode. When motion has ended and the telescope
         reaches the preset point, it returns to Stop mode and ends.
 
         Parameters:
-            az (float): destination angle for the azimuthal axis
+            az (float): destination angle for the azimuth axis
             el (float): destination angle for the elevation axis
             end_stop (bool): put the telescope in Stop mode at the end of
                 the motion
@@ -1300,7 +1327,8 @@ class ACUAgent:
             self.log.info(f'Requested position: az={target_az}, el={target_el}')
             session.set_status('running')
 
-            legs, msg = yield self._get_sunsafe_moves(target_az, target_el)
+            legs, msg = yield self._get_sunsafe_moves(target_az, target_el,
+                                                      zero_legs_ok=False)
             if msg is not None:
                 self.log.error(msg)
                 return False, msg
@@ -1322,7 +1350,7 @@ class ACUAgent:
     @ocs_agent.param('end_stop', default=False, type=bool)
     @inlineCallbacks
     def set_boresight(self, session, params):
-        """set_boresight(target=None, end_stop=False)
+        """set_boresight(target, end_stop=False)
 
         **Task** - Move the telescope to a particular third-axis angle.
 
@@ -1362,6 +1390,33 @@ class ACUAgent:
 
         return ok, msg
 
+    @ocs_agent.param('target')
+    @ocs_agent.param('end_stop', default=True, type=bool)
+    @inlineCallbacks
+    def go_to_named(self, session, params):
+        """go_to_named(target, end_stop=True)
+
+        **Task** - Move the telescope to a named position,
+        e.g. "home", that has been configured through command line args.
+
+        Parameters:
+          target (str): name of the target position.
+          end_stop (bool): put axes in Stop mode after motion
+
+        """
+        target = self.named_positions.get(params['target'])
+        if target is None:
+            return False, 'Position "%s" is not configured.' % params['target']
+
+        session.set_status('running')
+
+        ok, msg, _session = self.agent.start('go_to', {'az': target[0], 'el': target[1],
+                                                       'end_stop': params['end_stop']})
+        if ok == ocs.ERROR:
+            return False, 'Failed to start go_to task.'
+        ok, msg, _session = yield self.agent.wait('go_to')
+        return (ok == ocs.OK), msg
+
     @ocs_agent.param('speed_mode', choices=['high', 'low'])
     @inlineCallbacks
     def set_speed_mode(self, session, params):
@@ -1429,6 +1484,7 @@ class ACUAgent:
         yield
         return True, 'Done'
 
+    @ocs_agent.param('_')
     @inlineCallbacks
     def clear_faults(self, session, params):
         """clear_faults()
@@ -1505,7 +1561,7 @@ class ACUAgent:
     @ocs_agent.param('azonly', type=bool, default=True)
     @inlineCallbacks
     def fromfile_scan(self, session, params=None):
-        """fromfile_scan(filename=None, adjust_times=True, azonly=True)
+        """fromfile_scan(filename, adjust_times=True, azonly=True)
 
         **Task** - Upload and execute a scan pattern from numpy file.
 
@@ -1696,7 +1752,6 @@ class ACUAgent:
             'az_start', 'az_drift']
             if params.get(k) is not None}
         el_speed = params.get('el_speed', 0.0)
-
         plan = sh.plan_scan(az_endpoint1, az_endpoint2,
                             el=el_endpoint1, v_az=az_speed, a_az=az_accel,
                             az_start=scan_params.get('az_start'))
@@ -1737,7 +1792,8 @@ class ACUAgent:
 
         # Seek to starting position
         self.log.info(f'Moving to start position, az={plan["init_az"]}, el={init_el}')
-        legs, msg = yield self._get_sunsafe_moves(plan['init_az'], init_el)
+        legs, msg = yield self._get_sunsafe_moves(plan['init_az'], init_el,
+                                                  zero_legs_ok=True)
         if msg is not None:
             self.log.error(msg)
             return False, msg
@@ -2217,13 +2273,15 @@ class ACUAgent:
     @ocs_agent.param('avoidance_radius', type=float, default=None)
     @ocs_agent.param('shift_sun_hours', type=float, default=None)
     def update_sun(self, session, params):
-        """update_sun(reset, enable, temporary_disable, escape, \
-                      avoidance_radius, shift_sun_hours)
+        """update_sun(reset=None, enable=None, temporary_disable=None,
+                      escape=None, avoidance_radius=None,
+                      shift_sun_hours=None)
 
         **Task** - Update Sun monitoring and avoidance parameters.
 
-        Args:
+        All arguments are optional.
 
+        Args:
           reset (bool): If True, reset all sun_params to the platform
             defaults.  (The "defaults" includes any overrides
             specified on Agent command line.)
@@ -2400,7 +2458,7 @@ class ACUAgent:
 
         return safe, msg
 
-    def _get_sunsafe_moves(self, target_az, target_el):
+    def _get_sunsafe_moves(self, target_az, target_el, zero_legs_ok=True):
         """Given a target position, find a Sun-safe way to get there.  This
         will either be a direct move, or else an ordered slew in az
         before el (or vice versa).
@@ -2411,6 +2469,11 @@ class ACUAgent:
         targets, ``[(az0, el0), (az1, el1) ...]``, terminating on
         ``(target_az, target_el)``.  msg is None in that case.
 
+        In the case that platform is already at the target position,
+        an empty list of legs will be returned unless zero_legs_ok is
+        False in which case a 1-entry list of legs is returned, with
+        the target position in it.
+
         When Sun avoidance is not enabled, this function returns as
         though the direct path to the target is a safe one.
 
@@ -2439,7 +2502,10 @@ class ACUAgent:
         if move is None:
             return None, 'No Sun-Safe moves could be identified!'
 
-        return list(move['moves'].nodes[1:]), None
+        legs = list(move['moves'].nodes)
+        if len(legs) == 1 and not zero_legs_ok:
+            return legs, None
+        return legs[1:], None
 
     @ocs_agent.param('starting_index', type=int, default=0)
     def exercise(self, session, params):
@@ -2586,6 +2652,9 @@ def add_agent_args(parser_in=None):
     pgroup.add_argument("--fov-radius", type=float,
                         help="Override the default field-of-view (radius in "
                         "degrees) for Sun avoidance purposes.")
+    pgroup.add_argument("--named-positions", nargs='+',
+                        help="Define named positions, for go_to_named; e.g. 'home=180,60'.")
+
     return parser_in
 
 
@@ -2603,7 +2672,8 @@ def main(args=None):
                  avoid_sun=args.avoid_sun,
                  fov_radius=args.fov_radius,
                  min_el=args.min_el,
-                 max_el=args.max_el)
+                 max_el=args.max_el,
+                 named_positions=args.named_positions)
 
     runner.run(agent, auto_reconnect=True)
 
diff --git a/socs/agents/acu/avoidance.py b/socs/agents/acu/avoidance.py
index a65b7b3..9bb96db 100644
--- a/socs/agents/acu/avoidance.py
+++ b/socs/agents/acu/avoidance.py
@@ -447,7 +447,7 @@ class SunTracker:
         # Include the direct path, but put in "worst case" details
         # based on all "confined" paths computed above.
         direct = dict(base)
-        direct['moves'] = MoveSequence(az0, el0, az1, el1)
+        direct['moves'] = MoveSequence(az0, el0, az1, el1, simplify=True)
         traj_info = self.check_trajectory(*direct['moves'].get_traj(), t=t)
         direct.update(traj_info)
         conf = [m for m in all_moves if m['travel_el_confined']]
@@ -679,6 +679,9 @@ class MoveSequence:
         No step in az or el will be greater than res.
 
         """
+        if len(self.nodes) == 1:
+            return np.array([self.nodes[0][0]]), np.array([self.nodes[0][1]])
+
         xx, yy = [], []
         for (x0, y0), (x1, y1) in self.get_legs():
             n = max(2, math.ceil(abs(x1 - x0) / res), math.ceil(abs(y1 - y0) / res))
diff --git a/socs/agents/acu/drivers.py b/socs/agents/acu/drivers.py
index 3398fdc..836f17c 100644
--- a/socs/agents/acu/drivers.py
+++ b/socs/agents/acu/drivers.py
@@ -296,6 +296,12 @@ def generate_constant_velocity_scan(az_endpoint1, az_endpoint2, az_speed,
     if az_endpoint1 == az_endpoint2:
         raise ValueError('Generator requires two different az endpoints!')
 
+    # Force the el_speed to 0.  It matters because an el_speed in
+    # ProgramTrack data that exceeds the ACU limits will cause the
+    # point to be rejected, even if there's no motion in el planned
+    # (which, at the time of this writing, there is not).
+    el_speed = 0.
+
     # Note that starting scan direction gets modified, below,
     # depending on az_start.
     increasing = az_endpoint2 > az_endpoint1
diff --git a/socs/agents/hwp_pcu/agent.py b/socs/agents/hwp_pcu/agent.py
index 0b9f220..f5c4d77 100644
--- a/socs/agents/hwp_pcu/agent.py
+++ b/socs/agents/hwp_pcu/agent.py
@@ -1,78 +1,78 @@
 import argparse
 import time
+from dataclasses import dataclass
+from queue import Queue
+
+import txaio
+from twisted.internet import defer
+
+txaio.use_twisted()
 
 from ocs import ocs_agent, site_config
-from ocs.ocs_twisted import TimeoutLock
-from twisted.internet import reactor
 
 import socs.agents.hwp_pcu.drivers.hwp_pcu as pcu
 
 
+class Actions:
+    class BaseAction:
+        def __post_init__(self):
+            self.deferred = defer.Deferred()
+            self.log = txaio.make_logger()
+
+    @dataclass
+    class SendCommand (BaseAction):
+        command: str
+
+
+def process_action(action, PCU: pcu.PCU):
+    """Process an action with PCU hardware"""
+    if isinstance(action, Actions.SendCommand):
+        off_channel = []
+        on_channel = []
+        if action.command == 'off':
+            off_channel = [0, 1, 2, 5, 6, 7]
+            on_channel = []
+        elif action.command == 'on_1':
+            off_channel = [5, 6, 7]
+            on_channel = [0, 1, 2]
+        elif action.command == 'on_2':
+            on_channel = [0, 1, 2, 5, 6, 7]
+            off_channel = []
+        elif action.command == 'stop':
+            on_channel = [1, 2, 5]
+            off_channel = [0, 6, 7]
+
+        action.log.info(f"Command: {action.command}")
+        action.log.info(f"  Off channels: {off_channel}")
+        action.log.info(f"  On channels: {on_channel}")
+        for i in off_channel:
+            PCU.relay_off(i)
+        for i in on_channel:
+            PCU.relay_on(i)
+
+        return dict(off_channel=off_channel, on_channel=on_channel)
+
+
 class HWPPCUAgent:
     """Agent to phase compensation improve the CHWP motor efficiency
 
     Args:
         agent (ocs.ocs_agent.OCSAgent): Instantiated OCSAgent class for this agent
         port (str): Path to USB device in '/dev/'
-
     """
 
     def __init__(self, agent, port):
         self.agent: ocs_agent.OCSAgent = agent
         self.log = agent.log
-        self.lock = TimeoutLock()
-        self.initialized = False
-        self.take_data = False
         self.port = port
-        self.status = 'off'
+        self.action_queue = Queue()
 
         agg_params = {'frame_length': 60}
         self.agent.register_feed(
             'hwppcu', record=True, agg_params=agg_params)
 
-    @ocs_agent.param('auto_acquire', default=False, type=bool)
-    @ocs_agent.param('force', default=False, type=bool)
-    def init_connection(self, session, params):
-        """init_connection(auto_acquire=False, force=False)
-
-        **Task** - Initialize connection to PCU
-        Controller.
-
-        Parameters:
-            auto_acquire (bool, optional): Default is False. Starts data
-                acquisition after initialization if True.
-            force (bool, optional): Force initialization, even if already
-                initialized. Defaults to False.
-
-        """
-        if self.initialized and not params['force']:
-            self.log.info("Connection already initialized. Returning...")
-            return True, "Connection already initialized"
-
-        with self.lock.acquire_timeout(3, job='init_connection') as acquired:
-            if not acquired:
-                self.log.warn(
-                    'Could not run init_connection because {} is already running'.format(self.lock.job))
-                return False, 'Could not acquire lock'
-
-            try:
-                self.PCU = pcu.PCU(port=self.port)
-                self.log.info('Connected to PCU')
-            except BrokenPipeError:
-                self.log.error('Could not establish connection to PCU')
-                reactor.callFromThread(reactor.stop)
-                return False, 'Unable to connect to PCU'
-
-        self.status = self.PCU.get_status()
-        self.initialized = True
-
-        # Start 'acq' Process if requested
-        if params['auto_acquire']:
-            self.agent.start('acq')
-
-        return True, 'Connection to PCU established'
-
-    @ocs_agent.param('command', default='off', type=str, choices=['off', 'on_1', 'on_2', 'hold'])
+    @defer.inlineCallbacks
+    @ocs_agent.param('command', default='off', type=str, choices=['off', 'on_1', 'on_2', 'stop'])
     def send_command(self, session, params):
         """send_command(command)
 
@@ -80,131 +80,68 @@ class HWPPCUAgent:
         off: The compensation phase is zero.
         on_1: The compensation phase is +120 deg.
         on_2: The compensation phase is -120 deg.
-        hold: Stop the HWP spin.
+        stop: Stop the HWP spin.
 
         Parameters:
-            command (str): set the operation mode from 'off', 'on_1', 'on_2' or 'hold'.
+            command (str): set the operation mode from 'off', 'on_1', 'on_2' or 'stop'.
 
         """
-        with self.lock.acquire_timeout(3, job='send_command') as acquired:
-            if not acquired:
-                self.log.warn('Could not send command because {} is already running'.format(self.lock.job))
-                return False, 'Could not acquire lock'
-
-            command = params['command']
-            if command == 'off':
-                off_channel = [0, 1, 2, 5, 6, 7]
-                for i in off_channel:
-                    self.PCU.relay_off(i)
-                self.status = 'off'
-                return True, 'Phase compensation is "off".'
-
-            elif command == 'on_1':
-                on_channel = [0, 1, 2]
-                off_channel = [5, 6, 7]
-                for i in on_channel:
-                    self.PCU.relay_on(i)
-                for i in off_channel:
-                    self.PCU.relay_off(i)
-                self.status = 'on_1'
-                return True, 'Phase compensation operates "on_1".'
-
-            elif command == 'on_2':
-                on_channel = [0, 1, 2, 5, 6, 7]
-                for i in on_channel:
-                    self.PCU.relay_on(i)
-                self.status = 'on_2'
-                return True, 'Phase compensation operates "on_2".'
-
-            elif command == 'hold':
-                on_channel = [0, 1, 2, 5]
-                off_channel = [6, 7]
-                for i in on_channel:
-                    self.PCU.relay_on(i)
-                for i in off_channel:
-                    self.PCU.relay_off(i)
-                self.status = 'hold'
-                return True, 'Phase compensation operates "hold".'
-
-            else:
-                return True, "Choose the command from 'off', 'on_1', 'on_2' and 'hold'."
-
-    def get_status(self, session, params):
-        """get_status()
-
-        **Task** - Return the status of the PCU.
-
+        action = Actions.SendCommand(**params)
+        self.action_queue.put(action)
+        session.data = yield action.deferred
+        return True, f"Set relays for cmd={action.command}"
+
+    def _process_actions(self, PCU: pcu.PCU):
+        while not self.action_queue.empty():
+            action = self.action_queue.get()
+            try:
+                self.log.info(f"Running action {action}")
+                res = process_action(action, PCU)
+                action.deferred.callback(res)
+            except Exception as e:
+                self.log.error(f"Error processing action: {action}")
+                action.deferred.errback(e)
+
+    def _get_and_publish_data(self, PCU: pcu.PCU, session):
+        now = time.time()
+        data = {'timestamp': now,
+                'block_name': 'hwppcu',
+                'data': {}}
+        status = PCU.get_status()
+        data['data']['status'] = status
+        self.agent.publish_to_feed('hwppcu', data)
+        session.data = {'status': status, 'last_updated': now}
+
+    def main(self, session, params):
         """
-        with self.lock.acquire_timeout(3, job='get_status') as acquired:
-            if not acquired:
-                self.log.warn(
-                    'Could not get status because {} is already running'.format(self.lock.job))
-                return False, 'Could not acquire lock'
-
-            self.status = self.PCU.get_status()
-
-        return True, 'Current status is ' + self.status
-
-    def acq(self, session, params):
-        """acq()
-
-        **Process** - Start PCU data acquisition.
-
-        Notes:
-            The most recent data collected is stored in the session data in the
-            structure::
-
-                >>> response.session['data']
-                {'status': 'on_1',
-                 'last_updated': 1649085992.719602}
-
+        **Process** - Main process for PCU agent.
         """
-        with self.lock.acquire_timeout(timeout=3, job='acq') as acquired:
-            if not acquired:
-                self.log.warn('Could not start pcu acq because {} is already running'
-                              .format(self.lock.job))
-                return False, 'Could not acquire lock'
-
-            session.set_status('running')
-            last_release = time.time()
-            self.take_data = True
-
-            while self.take_data:
-                # Relinquish sampling lock occasionally.
-                if time.time() - last_release > 1.:
-                    last_release = time.time()
-                    if not self.lock.release_and_acquire(timeout=10):
-                        self.log.warn(f"Failed to re-acquire sampling lock, "
-                                      f"currently held by {self.lock.job}.")
-                        continue
+        PCU = pcu.PCU(port=self.port)
+        self.log.info('Connected to PCU')
 
-                data = {'timestamp': time.time(),
-                        'block_name': 'hwppcu', 'data': {}}
+        session.set_status('running')
+        while not self.action_queue.empty():
+            action = self.action_queue.get()
+            action.deferred.errback(Exception("Action cancelled"))
 
-                # status = self.PCU.get_status()
-                status = self.status
-                data['data']['status'] = status
+        last_daq = 0
+        while session.status in ['starting', 'running']:
+            now = time.time()
+            if now - last_daq > 5:
+                self._get_and_publish_data(PCU, session)
+                last_daq = now
 
-                self.agent.publish_to_feed('hwppcu', data)
+            self._process_actions(PCU)
+            time.sleep(0.1)
 
-                session.data = {'status': status,
-                                'last_updated': time.time()}
+        PCU.close()
 
-                time.sleep(5)
-
-        self.agent.feeds['hwppcu'].flush_buffer()
-        return True, 'Acqusition exited cleanly'
-
-    def _stop_acq(self, session, params):
+    def _stop_main(self, session, params):
         """
         Stop acq process.
         """
-        if self.take_data:
-            self.PCU.close()
-            self.take_data = False
-            return True, 'requested to stop taking data'
-
-        return False, 'acq is not currently running'
+        session.set_status('stopping')
+        return True, 'Set main status to stopping'
 
 
 def make_parser(parser=None):
@@ -217,10 +154,7 @@ def make_parser(parser=None):
 
     # Add options specific to this agent
     pgroup = parser.add_argument_group('Agent Options')
-    pgroup.add_argument('--port', type=str, help="Path to USB node for the lakeshore")
-    pgroup.add_argument('--mode', type=str, default='acq',
-                        choices=['init', 'acq'],
-                        help="Starting operation for the Agent.")
+    pgroup.add_argument('--port', type=str, help="Path to USB node for the PCU")
     return parser
 
 
@@ -230,21 +164,12 @@ def main(args=None):
                                   parser=parser,
                                   args=args)
 
-    init_params = False
-    if args.mode == 'init':
-        init_params = {'auto_acquire': False}
-    elif args.mode == 'acq':
-        init_params = {'auto_acquire': True}
-
     agent, runner = ocs_agent.init_site_agent(args)
     hwppcu_agent = HWPPCUAgent(agent,
                                port=args.port)
-    agent.register_task('init_connection', hwppcu_agent.init_connection,
-                        startup=init_params)
-    agent.register_process('acq', hwppcu_agent.acq,
-                           hwppcu_agent._stop_acq)
-    agent.register_task('send_command', hwppcu_agent.send_command)
-    agent.register_task('get_status', hwppcu_agent.get_status)
+    agent.register_task('send_command', hwppcu_agent.send_command, blocking=False)
+    agent.register_process(
+        'main', hwppcu_agent.main, hwppcu_agent._stop_main, startup=True)
 
     runner.run(agent, auto_reconnect=True)
 
diff --git a/socs/agents/hwp_pcu/drivers/hwp_pcu.py b/socs/agents/hwp_pcu/drivers/hwp_pcu.py
index 7ea7e58..86132c8 100644
--- a/socs/agents/hwp_pcu/drivers/hwp_pcu.py
+++ b/socs/agents/hwp_pcu/drivers/hwp_pcu.py
@@ -10,7 +10,7 @@ class PCU:
         port (str): Path to USB device in '/dev/'
 
     Attributes:
-        status (str): The status of the unit (off/on_1/on_2/hold)
+        status (str): The status of the unit (off/on_1/on_2/stop)
     """
 
     def __init__(self, port):
@@ -54,7 +54,7 @@ class PCU:
         off: The compensation phase is zero.
         on_1: The compensation phase is +120 deg.
         on_2: The compensation phase is -120 deg.
-        hold: Stop the HWP spin.
+        stop: Stop the HWP spin.
         """
         channel = [0, 1, 2, 5, 6, 7]
         channel_switch = []
@@ -67,7 +67,9 @@ class PCU:
             return 'on_1'
         elif channel_switch == [True, True, True, True, True, True]:
             return 'on_2'
-        elif channel_switch == [True, True, True, True, False, False]:
-            return 'hold'
+        elif channel_switch == [False, True, True, True, False, False]:
+            return 'stop'
+        elif -1 in channel_switch:
+            return 'failed'
         else:
             return 'undefined'


Test patch:
diff --git a/tests/agents/test_acu_agent.py b/tests/agents/test_acu_agent.py
index 9afa6f2..dc928c3 100644
--- a/tests/agents/test_acu_agent.py
+++ b/tests/agents/test_acu_agent.py
@@ -21,6 +21,24 @@ def test_avoidance():
     assert sun.check_trajectory([90], [20])['sun_time'] > 0
     assert sun.check_trajectory([270], [60])['sun_time'] > 0
 
+    # Find safe paths
+    paths = sun.analyze_paths(180, 30, 270, 40)
+    path, analysis = sun.select_move(paths)
+    assert path is not None
+    assert len(path['moves'].nodes) > 1
+
+    # Find safe short paths
+    paths = sun.analyze_paths(270.01, 40.01, 270, 40)
+    path, analysis = sun.select_move(paths)
+    assert path is not None
+    assert len(path['moves'].nodes) == 2
+
+    # Find safe paths to here (no moves)
+    paths = sun.analyze_paths(270, 40, 270, 40)
+    path, analysis = sun.select_move(paths)
+    assert path is not None
+    assert len(path['moves'].nodes) == 1
+
     # No safe moves to Sun position.
     paths = sun.analyze_paths(180, 20, az0, el0)
     path, analysis = sun.select_move(paths)