diff --git a/docs/agents/hwp_supervisor_agent.rst b/docs/agents/hwp_supervisor_agent.rst index 56a2aee52..5d84032cd 100644 --- a/docs/agents/hwp_supervisor_agent.rst +++ b/docs/agents/hwp_supervisor_agent.rst @@ -35,6 +35,8 @@ Here is an example of a config block you can add to your ocs site-config file:: '--hwp-encoder-id', 'hwp-bbb-e1', '--hwp-pmx-id', 'hwp-pmx', '--hwp-pid-id', 'hwp-pid', + '--hwp-pcu-id', 'hwp-pcu', + '--hwp-gripper-id', 'hwp-gripper', '--ups-id', 'power-ups-az', '--ups-minutes-remaining-thresh', 45, '--iboot-id', 'power-iboot-hwp-2', @@ -43,6 +45,13 @@ Here is an example of a config block you can add to your ocs site-config file:: '--driver-power-agent-type', 'synaccess', '--driver-power-cycle-twice', '--driver-power-cycle-wait-time', 300, + + '--acu-instance-id', 'acu', + '--acu-min-el', 48.0, + '--acu-max-el', 90.0, + '--acu-max-time-since-update', 30.0, + '--mount-velocity-grip-thresh', 0.005, + '--grip-max-boresight-angle', 1.0, ]} For SATP1, we use an ibootbar to power the driver, and it is not necessary to @@ -57,12 +66,20 @@ cycle the driver power twice, so the config will look like:: '--hwp-encoder-id', 'hwp-bbb-e1', '--hwp-pmx-id', 'hwp-pmx', '--hwp-pid-id', 'hwp-pid', + '--hwp-pcu-id', 'hwp-pcu', + '--hwp-gripper-id', 'hwp-gripper', '--ups-id', 'power-ups-az', '--ups-minutes-remaining-thresh', 45, '--iboot-id', 'power-iboot-hwp-2', '--driver-iboot-id', 'power-iboot-hwp-2', '--driver-iboot-outlets', 1, 2, '--driver-power-agent-type', 'iboot', + + '--acu-instance-id', 'acu', + '--acu-min-el', 48.0, + '--acu-max-el', 90.0, + '--acu-max-time-since-update', 30.0, + '--mount-velocity-grip-thresh', 0.005, ]} .. note:: @@ -140,6 +157,11 @@ Before spinning up the HWP, the agent will check that - The current elevation and commanded elevation are in the range ``(acu-min-el, acu-max-el)``. - The ACU state info has been updated within ``acu-max-time-since-update`` seconds. +Before gripping or ungripping the HWP, the agent will check that: +- The current elevation and commanded elevation are in the range ``(acu-min-el, acu-max-el)``. +- The ACU state info has been updated within ``acu-max-time-since-update`` seconds. +- The az and el velocity is less than ``mount-velocity-grip-thresh``. + Examples ``````````` Below is an example client script that runs the PID to freq operation, and waits diff --git a/socs/agents/hwp_supervisor/agent.py b/socs/agents/hwp_supervisor/agent.py index 06c7a1b89..3957b1262 100644 --- a/socs/agents/hwp_supervisor/agent.py +++ b/socs/agents/hwp_supervisor/agent.py @@ -3,18 +3,19 @@ import threading import time import traceback +from contextlib import contextmanager from dataclasses import asdict, dataclass, field -from typing import Dict, List, Literal, Optional +from typing import Any, Dict, Generator, List, Literal, Optional import numpy as np import ocs import txaio from ocs import client_http, ocs_agent, site_config -from ocs.client_http import ControlClientError +from ocs.client_http import ControlClient, ControlClientError from ocs.ocs_client import OCSClient, OCSReply from ocs.ocs_twisted import Pacemaker -client_cache = {} +client_cache: Dict[str, ControlClient] = {} def get_op_data(agent_id, op_name, log=None, test_mode=False): @@ -112,6 +113,7 @@ class HWPClients: gripper_iboot: Optional[OCSClient] = None driver_iboot: Optional[OCSClient] = None pcu: Optional[OCSClient] = None + gripper: Optional[OCSClient] = None @dataclass @@ -166,6 +168,11 @@ class ACUState: Maximum elevation allowed before restricting spin-up [deg] max_time_since_update : float Maximum time since last update before restricting spin-up[sec] + mount_velocity_grip_thresh: float + Maximum mount velocity for gripping the HWP [deg/s] . + grip_max_boresight_angle: float + Maximum absolute boresight angle [deg] for which operating the grippers + is allowed. Attributes ------------ @@ -175,18 +182,42 @@ class ACUState: Commanded el position [deg] el_current_velocity : float Current el velocity [deg/s] + az_current_velocity : float + Current az velocity [deg/s] + bor_current_position : float + Current bor position [deg] + bor_commanded_position : float + Commanded bor position [deg] last_updated : float - Time of last update [sec] + Time of last update [sec]. If this is None, you cannot trust other + state variables. """ instance_id: str min_el: float max_el: float max_time_since_update: float + mount_velocity_grip_thresh: float + grip_max_boresight_angle: float - el_current_position: Optional[float] = None - el_commanded_position: Optional[float] = None - el_current_velocity: Optional[float] = None last_updated: Optional[float] = None + el_current_position: float = 0.0 + el_commanded_position: float = 0.0 + el_current_velocity: float = 0.0 + az_current_velocity: float = 0.0 + bor_current_position: float = 0.0 + bor_commanded_position: float = 0.0 + + # This will only be set by spin-control agent for communication with ACU + request_block_motion: bool = False + request_block_motion_timestamp: float = 0.0 + + ACU_motion_blocked: Optional[bool] = None # This flag is only set by the ACU agent + use_acu_blocking: bool = False + block_motion_timeout: float = 60.0 + + def set_request_block_motion(self, state: bool) -> None: + self.request_block_motion = state + self.request_block_motion_timestamp = time.time() def update(self): op = get_op_data(self.instance_id, 'monitor') @@ -200,6 +231,10 @@ def update(self): self.el_current_position = d['Elevation current position'] self.el_commanded_position = d['Elevation commanded position'] self.el_current_velocity = d['Elevation current velocity'] + self.az_current_velocity = d['Azimuth current velocity'] + self.bor_current_position = d['Boresight current position'] + self.bor_commanded_position = d['Boresight commanded position'] + t = d.get('timestamp_agent') if t is None: t = time.time() @@ -226,6 +261,7 @@ class HWPState: pid_target_freq: Optional[float] = None pid_direction: Optional[str] = None pid_last_updated: Optional[float] = None + pid_max_time_since_update: float = 60.0 pmx_current: Optional[float] = None pmx_voltage: Optional[float] = None @@ -241,6 +277,8 @@ class HWPState: acu: Optional[ACUState] = None + supervisor_control_state: Optional[Dict[str, Any]] = None + @classmethod def from_args(cls, args: argparse.Namespace): log = txaio.make_logger() # pylint: disable=E1101 @@ -248,6 +286,7 @@ def from_args(cls, args: argparse.Namespace): temp_field=args.ybco_temp_field, temp_thresh=args.ybco_temp_thresh, ups_minutes_remaining_thresh=args.ups_minutes_remaining_thresh, + pid_max_time_since_update=args.pid_max_time_since_update, ) if args.gripper_iboot_id is not None: @@ -264,17 +303,21 @@ def from_args(cls, args: argparse.Namespace): else: log.warn("Driver Ibootbar id not set") - if args.acu_instance_id is not None: + if not args.no_acu: self.acu = ACUState( instance_id=args.acu_instance_id, min_el=args.acu_min_el, max_el=args.acu_max_el, - max_time_since_update=args.acu_max_time_since_update + max_time_since_update=args.acu_max_time_since_update, + mount_velocity_grip_thresh=args.mount_velocity_grip_thresh, + grip_max_boresight_angle=args.grip_max_boresight_angle, + use_acu_blocking=args.use_acu_blocking, + block_motion_timeout=args.acu_block_motion_timeout, ) log.info("ACU state checking enabled: instance_id={id}", id=self.acu.instance_id) else: - log.info("ACU state checking disabled.") + log.warn("The no-acu option has been set. ACU state checking disabled.") return self @@ -626,6 +669,18 @@ class PmxOff: """ success: bool = True + @dataclass + class GripHWP: + """ + Grips the HWP + """ + + @dataclass + class UngripHWP: + """ + Ungrips the HWP + """ + @dataclass class Abort: """Abort current action""" @@ -750,8 +805,101 @@ def sleep_until_complete(self, session=None, dt=1): time.sleep(dt) +@contextmanager +def ensure_grip_safety(hwp_state: HWPState, log: txaio.ILogger) -> Generator[None, None, None]: + """ + Run required checks for gripper safety. This will check ACU parameters such + as az/el position and velocity. If `use_acu_blocking` is set, this will + also attempt to block out ACU motion for the duration of the operation. + + Args + ------ + hwp_state : HWPState + HWP state object. In addition to reading ACU state vars, this will + set the `request_block_ACU_motion` flag if `use_acu_blocking` is set. + timeout: float + Timeout for waiting for the ACU blockout before an error will be raised. + """ + now = time.time() + + if hwp_state.pid_current_freq is None or hwp_state.pid_last_updated is None: + raise RuntimeError("Cannot determine current HWP Freq") + + tdiff = now - hwp_state.pid_last_updated + if tdiff > hwp_state.pid_max_time_since_update: + raise RuntimeError(f"HWP PID state has not been updated in {tdiff} sec") + + if np.abs(hwp_state.pid_current_freq) > 0.02: + raise RuntimeError("Cannot grip HWP while spinning") + log.info("Rotation safety checks have passed") + + acu = hwp_state.acu + if acu is None: + yield + return + + # Run checks of ACU state + if acu.last_updated is None: + raise RuntimeError( + f"No ACU data has been received from instance-id {acu.instance_id}" + ) + + tdiff = time.time() - acu.last_updated + if tdiff > acu.max_time_since_update: + raise RuntimeError(f"ACU state has not been updated in {tdiff} sec") + + if not (acu.min_el <= acu.el_current_position <= acu.max_el): + raise RuntimeError( + f"ACU elevation is {acu.el_current_position} deg, " + f"outside of allowed range ({acu.min_el}, {acu.max_el})" + ) + if not (acu.min_el <= acu.el_commanded_position <= acu.max_el): + raise RuntimeError( + f"ACU commanded elevation is {acu.el_commanded_position} deg, " + f"outside of allowed range ({acu.min_el}, {acu.max_el})" + ) + if np.abs(acu.az_current_velocity) > np.abs(acu.mount_velocity_grip_thresh): + raise RuntimeError( + f"ACU az-velocity is {acu.az_current_velocity} deg/s, " + f"above threshold of {acu.mount_velocity_grip_thresh} deg/s" + ) + if np.abs(acu.el_current_velocity) > np.abs(acu.mount_velocity_grip_thresh): + raise RuntimeError( + f"ACU el-velocity is {acu.el_current_velocity} deg/s, " + f"above threshold of {acu.mount_velocity_grip_thresh} deg/s" + ) + if np.abs(acu.bor_current_position) > np.abs(acu.grip_max_boresight_angle): + raise RuntimeError( + f"boresight current angle of {acu.bor_current_position} deg, " + f"above threshold of {acu.grip_max_boresight_angle} deg" + ) + if np.abs(acu.bor_commanded_position) > np.abs(acu.grip_max_boresight_angle): + raise RuntimeError( + f"boresight commanded angle of {acu.bor_commanded_position} deg, " + f"above threshold of {acu.grip_max_boresight_angle} deg" + ) + log.info("ACU safety checks have passed") + + if not acu.use_acu_blocking: + yield + return + + try: # If use_acu_blocking, do handshake with ACU agent to block motino + acu.set_request_block_motion(True) + log.info("Requesting ACU to block motion") + while not acu.ACU_motion_blocked: + if time.time() - acu.request_block_motion_timestamp > acu.block_motion_timeout: + raise RuntimeError("ACU motion was not blocked within timeout") + time.sleep(1) + log.info("ACU motion has been blocked") + yield + finally: + log.info("Releasing ACU motion block") + acu.set_request_block_motion(False) + + class ControlStateMachine: - def __init__(self): + def __init__(self) -> None: self.action: ControlAction = ControlAction(ControlState.Idle()) self.action_history: List[ControlAction] = [] self.max_action_history_count = 100 @@ -824,7 +972,7 @@ def check_acu_ok_for_spinup(): if tdiff > acu.max_time_since_update: raise RuntimeError(f"ACU state has not been updated in {tdiff} sec") if not (acu.min_el <= acu.el_current_position <= acu.max_el): - raise RuntimeError(f"ACU elevation is {acu.el_current_pos} deg, " + raise RuntimeError(f"ACU elevation is {acu.el_current_position} deg, " f"outside of allowed range ({acu.min_el}, {acu.max_el})") if not (acu.min_el <= acu.el_commanded_position <= acu.max_el): raise RuntimeError(f"ACU commanded elevation is {acu.el_commanded_position} deg, " @@ -945,6 +1093,16 @@ def check_acu_ok_for_spinup(): ) self.action.set_state(ControlState.Done(success=state.success)) + elif isinstance(state, ControlState.GripHWP): + with ensure_grip_safety(hwp_state, self.log): + self.run_and_validate(clients.gripper.grip) + self.action.set_state(ControlState.Done(success=True)) + + elif isinstance(state, ControlState.UngripHWP): + with ensure_grip_safety(hwp_state, self.log): + self.run_and_validate(clients.gripper.ungrip) + self.action.set_state(ControlState.Done(success=True)) + elif isinstance(state, ControlState.Brake): self.run_and_validate( clients.pcu.send_command, @@ -1122,6 +1280,7 @@ def get_client(id): lakeshore=get_client(self.ybco_lakeshore_id), gripper_iboot=get_client(self.gripper_iboot_id), driver_iboot=get_client(self.driver_iboot_id), + gripper=get_client(self.args.hwp_gripper_id), ) @ocs_agent.param('test_mode', type=bool, default=False) @@ -1269,13 +1428,24 @@ def spin_control(self, session, params): """ clients = self._get_hwp_clients() + def update_supervisor_state(): + state_info = self.control_state_machine.action.cur_state_info.encode() + self.hwp_state.supervisor_control_state = state_info + if session.data: + session.data['current_state_type'] = self.control_state_machine.action.cur_state_info.state_type + while session.status in ['starting', 'running']: + # Update session data beginning and end of update call, because its + # possible for an action change to happen due to external request from + # task. + update_supervisor_state() self.control_state_machine.update(clients, self.hwp_state) - session.data = { + update_supervisor_state() + session.data.update({ 'current_action': self.control_state_machine.action.encode(), 'action_history': [a.encode() for a in self.control_state_machine.action_history], - 'timestamp': time.time() - } + 'timestamp': time.time(), + }) if params['test_mode']: break time.sleep(1) @@ -1441,6 +1611,54 @@ def pmx_off(self, session, params): action.sleep_until_complete(session=session) return action.success, f"Completed with state: {action.cur_state_info.state}" + def grip_hwp(self, session, params): + """grip_hwp() + + **Task** - Grip the HWP if ACU and HWP state passes checks. + + Notes + -------- + + Example of ``session.data``:: + + >>> session['data'] + {'action': + {'action_id': 3, + 'completed': True, + 'cur_state': {'class': 'Done', 'msg': None, 'success': True}, + 'state_history': List[ConrolState], + 'success': True} + } + """ + state = ControlState.GripHWP() + action = self.control_state_machine.request_new_action(state) + action.sleep_until_complete(session=session) + return action.success, f"Completed with state: {action.cur_state_info.state}" + + def ungrip_hwp(self, session, params): + """ungrip_hwp() + + **Task** - Ungrip the HWP if ACU and HWP state passes checks. + + Notes + -------- + + Example of ``session.data``:: + + >>> session['data'] + {'action': + {'action_id': 3, + 'completed': True, + 'cur_state': {'class': 'Done', 'msg': None, 'success': True}, + 'state_history': List[ConrolState], + 'success': True} + } + """ + state = ControlState.UngripHWP() + action = self.control_state_machine.request_new_action(state) + action.sleep_until_complete(session=session) + return action.success, f"Completed with state: {action.cur_state_info.state}" + def abort_action(self, session, params): """abort_action() @@ -1544,10 +1762,16 @@ def make_parser(parser=None): help="Instance ID for HWP pid agent") pgroup.add_argument('--hwp-pcu-id', help="Instance ID for HWP PCU agent") + pgroup.add_argument('--hwp-gripper-id', + help="Instance ID for HWP Gripper agent") pgroup.add_argument('--ups-id', help="Instance ID for UPS agent") pgroup.add_argument('--ups-minutes-remaining-thresh', type=float, help="Threshold for UPS minutes remaining before a " "shutdown is triggered") + pgroup.add_argument( + '--pid-max-time-since-update', type=float, default=60.0, + help="Max amount of time since last PID update before data is considered stale.", + ) pgroup.add_argument( '--driver-iboot-id', @@ -1576,9 +1800,13 @@ def make_parser(parser=None): help="Type of agent used for controlling the gripper power") pgroup.add_argument( - '--acu-instance-id', - help="Instance ID for the ACU agent. This is required for checks of ACU " - "postiion and velocity before HWP commands." + '--acu-instance-id', default='acu', + help="Instance ID for the ACU agent." + ) + pgroup.add_argument( + '--no-acu', action='store_true', + help="If set, will not attempt to connect to the ACU or perform any ACU " + "checks for grip and spin-up" ) pgroup.add_argument( '--acu-min-el', type=float, default=48.0, @@ -1592,6 +1820,23 @@ def make_parser(parser=None): '--acu-max-time-since-update', type=float, default=30.0, help="Max amount of time since last ACU update before allowing HWP spin up", ) + pgroup.add_argument( + '--mount-velocity-grip-thresh', type=float, default=0.005, + help="Max mount velocity (both az and el) for gripping the HWP" + ) + pgroup.add_argument( + '--grip-max-boresight-angle', type=float, default=1., + help="Maximum absolute value of boresight angle (deg) for gripping the HWP" + ) + pgroup.add_argument( + '--use-acu-blocking', action='store_true', + help="If True, will use blocking flags to make sure ACU is not moving " + "during HWP gripper commands." + ) + pgroup.add_argument( + '--acu-block-motion-timeout', type=float, default=60., + help="Time to wait for ACU motion to be blocked before aborting gripper command." + ) pgroup.add_argument('--forward-dir', choices=['cw', 'ccw'], default="cw", help="Whether the PID 'forward' direction is cw or ccw") @@ -1615,9 +1860,11 @@ def main(args=None): agent.register_task('set_const_voltage', hwp.set_const_voltage) agent.register_task('brake', hwp.brake) agent.register_task('pmx_off', hwp.pmx_off) - agent.register_task('abort_action', hwp.abort_action) + agent.register_task('grip_hwp', hwp.grip_hwp) + agent.register_task('ungrip_hwp', hwp.ungrip_hwp) agent.register_task('enable_driver_board', hwp.enable_driver_board) agent.register_task('disable_driver_board', hwp.disable_driver_board) + agent.register_task('abort_action', hwp.abort_action) runner.run(agent, auto_reconnect=True) diff --git a/socs/agents/scpi_psu/agent.py b/socs/agents/scpi_psu/agent.py index f89a352fb..11e235f90 100644 --- a/socs/agents/scpi_psu/agent.py +++ b/socs/agents/scpi_psu/agent.py @@ -1,6 +1,7 @@ import argparse import socket import time +from typing import Optional from ocs import ocs_agent, site_config from ocs.ocs_twisted import TimeoutLock @@ -19,7 +20,7 @@ def __init__(self, agent, ip_address, gpib_slot): self.gpib_slot = gpib_slot self.monitor = False - self.psu = None + self.psu: Optional[ScpiPsuAgent] = None # Registers Temperature and Voltage feeds agg_params = { @@ -41,16 +42,25 @@ def init(self, session, params=None): if not acquired: return False, "Could not acquire lock" - try: - self.psu = PsuInterface(self.ip_address, self.gpib_slot) - self.idn = self.psu.identify() - except socket.timeout as e: - self.log.error(f"PSU timed out during connect: {e}") - return False, "Timeout" - self.log.info("Connected to psu: {}".format(self.idn)) - + while not self._initialize_module(): + time.sleep(5) return True, 'Initialized PSU.' + def _initialize_module(self): + """Initialize the ScpiPsu module.""" + try: + self.psu = PsuInterface(self.ip_address, self.gpib_slot) + except (socket.timeout, OSError) as e: + self.log.warn(f"Error establishing connection: {e}") + self.psu = None + return False + + self.idn = self.psu.identify() + self.log.info("Connected to psu: {}".format(self.idn)) + self.log.info("Clearing event registers and error queue") + self.psu.clear() + return True + @ocs_agent.param('wait', type=float, default=1) @ocs_agent.param('channels', type=list, default=[1, 2, 3]) @ocs_agent.param('test_mode', type=bool, default=False) @@ -71,29 +81,36 @@ def monitor_output(self, session, params=None): self.monitor = True while self.monitor: + time.sleep(params['wait']) with self.lock.acquire_timeout(1) as acquired: - if acquired: - data = { - 'timestamp': time.time(), - 'block_name': 'output', - 'data': {} - } + if not acquired: + self.log.warn("Could not acquire in monitor_current") + continue + + if not self.psu: + self._initialize_module() + continue + data = { + 'timestamp': time.time(), + 'block_name': 'output', + 'data': {} + } + + try: for chan in params['channels']: data['data']["Voltage_{}".format(chan)] = self.psu.get_volt(chan) data['data']["Current_{}".format(chan)] = self.psu.get_curr(chan) + except socket.timeout as e: + self.log.warn(f"TimeoutError: {e}") + self.log.info("Attempting to reconnect") + self.psu = None + continue - # self.log.info(str(data)) - # print(data) - self.agent.publish_to_feed('psu_output', data) - - # Allow this process to be queried to return current data - session.data = data + self.agent.publish_to_feed('psu_output', data) - else: - self.log.warn("Could not acquire in monitor_current") - - time.sleep(params['wait']) + # Allow this process to be queried to return current data + session.data = data if params['test_mode']: break diff --git a/socs/agents/scpi_psu/drivers.py b/socs/agents/scpi_psu/drivers.py index 64a1dbead..e986e43d0 100644 --- a/socs/agents/scpi_psu/drivers.py +++ b/socs/agents/scpi_psu/drivers.py @@ -80,3 +80,9 @@ def get_curr(self, ch): self.write('MEAS:CURR? CH' + str(ch)) current = float(self.read()) return current + + def clear(self): + # Clear all the event registers and error queue, using a query such as *ESR? or MEAS:X? + # instead of *CLS can confuse the PSU + self.write('*CLS') + return True