diff --git a/config/hsfei/hsfei_hkpiaagim.yaml b/config/hsfei/hsfei_hkpiaagim.yaml new file mode 100644 index 0000000..524042d --- /dev/null +++ b/config/hsfei/hsfei_hkpiaagim.yaml @@ -0,0 +1,35 @@ +# +# Usage: +# daemons/hsfei/piaa-gimbalmount -c config/hsfei/hsfei_hkpiaagim.yaml + +peer_id: hsfei_hkpiaagim +group_id: hsfei + +hardware: + ip_address: 192.168.29.100 + tcp_port: 10012 + axis: 2 + closed_loop_units: mrad + open_loop_units: volts + timeout: 30.0 + retry_count: 3 + +limits: + open_loop: + soft_min: -20.0 + soft_max: 145.0 + hard_min: -25.0 + hard_max: 150.0 + closed_loop: + soft_min: -9.5 + soft_max: 9.5 + hard_min: -10.0 + hard_max: 10.0 + +named_positions: + center: [0.0, 0.0] + offset1: [1.0, 1.0] + offset2: [-1.0, -1.0] + +logging: + level: INFO \ No newline at end of file diff --git a/config/hsfei/hsfei_yjpiaagim.yaml b/config/hsfei/hsfei_yjpiaagim.yaml new file mode 100644 index 0000000..339a33d --- /dev/null +++ b/config/hsfei/hsfei_yjpiaagim.yaml @@ -0,0 +1,35 @@ +# +# Usage: +# daemons/hsfei/piaa-gimbalmount -c config/hsfei/hsfei_yjpiaagim.yaml + +peer_id: hsfei_yjpiaagim +group_id: hsfei + +hardware: + ip_address: 192.168.29.100 + tcp_port: 10013 + axis: 2 + closed_loop_units: mrad + open_loop_units: volts + timeout: 30.0 + retry_count: 3 + +limits: + open_loop: + soft_min: -20.0 + soft_max: 145.0 + hard_min: -25.0 + hard_max: 150.0 + closed_loop: + soft_min: -9.5 + soft_max: 9.5 + hard_min: -10.0 + hard_max: 10.0 + +named_positions: + center: [0.0, 0.0] + offset1: [1.0, 1.0] + offset2: [-1.0, -1.0] + +logging: + level: INFO \ No newline at end of file diff --git a/daemons/hsfei/piaa-gimbalmount b/daemons/hsfei/piaa-gimbalmount new file mode 100755 index 0000000..b8fbe07 --- /dev/null +++ b/daemons/hsfei/piaa-gimbalmount @@ -0,0 +1,449 @@ +#!/usr/bin/env python3 +'''Module for Gimbal mount Daemon''' +import argparse +import sys +from typing import Dict, Any, Optional #pylint: disable = W0611 + +from hispec.daemon import HispecDaemon #pylint: disable = E0611 +from hispec.driver.thorlabs.ppc102 import Ppc102Controller #pylint: disable = E0611 +#from ppc102 import Ppc102Controller # Assuming ppc102.py is in the same directory + +class PiaaGimbalmount(HispecDaemon): #pylint: disable = W0223 + '''Daemon for controlling the Blue Piaa Gimbal Mount via Thorlabs PPC102 controller''' + + # Defaults + group_id = "hsfei" + + # pub/sub topics + topics = {} + + #voltage control parameters + V_MIN = -20 + V_MAX = 150 + DIGITAL_MIN = -32767 + DIGITAL_MAX = 32767 + + def __init__(self): + """Initialize the Gimbal daemon.""" + super().__init__() + + #Defaults + self.host = None + self.port = None + self.dev = Ppc102Controller(log = True) + self.daemon_desc = None + self.units = None + self._soft_min = None + self._soft_max = None + self._hard_min = None + self._hard_max = None + self.named_positions = None + + # Daemon state + self.state = { + 'connected': False, + 'error': '', + 'enabled': False, + 'isloopsclosed': False + } + + def on_start(self, libby): + '''Starts up daemon and initializies the hardware device''' + + #load configuration from hsfei.yaml + self.host = self.get_config("hardware.ip_address") + self.port = self.get_config("hardware.tcp_port") + self.daemon_desc = self.get_config("peer_id") + self.units = self.get_config("hardware.open_loop_units") + self._soft_min = self.get_config("limits.open_loop.soft_min") + self._soft_max = self.get_config("limits.open_loop.soft_max") + self._hard_min = self.get_config("limits.open_loop.hard_min") + self._hard_max = self.get_config("limits.open_loop.hard_max") + self.named_positions = self.get_config("named_positions") + + # Initialize hardware connection + if not(self.host and self.port): + self.logger.error("No IP address or port specified for Filter Wheel controller") + self.state['error'] = 'No IP address or port specified' + return + + try: + connection = self.connect(True) + if not connection.get("ok"): + raise ConnectionError(connection.get("error")) + self.state['connected'] = True + self.initialize() + self.keyword_registry.bool("isconnected", + getter=self.dev.is_connected, + setter=self.keyword_wrapper(self.connect, key="isconnected"), + description="Check if daemon can talk to the GimbalMount controller.") + self.keyword_registry.bool("isloopsclosed", + getter=self.dev.is_loop_closed, + setter=self.keyword_wrapper(self.set_loops, key="isloopsclosed"), + description="Check if gimbal loops are closed.") + self.keyword_registry.float("positionvaluex", + getter=self.keyword_wrapper(self.get_xpos, key="position"), + setter=self.keyword_wrapper(self.set_xpos, key="position"), + validator=self._check_soft_limits, + units=self.units, + description="Set and get current position of GimbalMount.") + self.keyword_registry.float("positionvaluey", + getter=self.keyword_wrapper(self.get_ypos, key="position"), + setter=self.keyword_wrapper(self.set_ypos, key="position"), + validator=self._check_soft_limits, + units=self.units, + description="Set and get current position of GimbalMount.") + self.keyword_registry.string("positionnamed", + getter=self.keyword_wrapper(self.cur_named_position, key="named_pos"), + setter=self.keyword_wrapper(self.goto_named_pos, key="named_pos"), + validator=self._check_named, + description="Set and get named position of GimbalMount.") + self.keyword_registry.int("softmin", + getter=lambda: self._soft_min, + setter=lambda v: setattr(self, "_soft_min", int(v)), + units=self.units, + description="Software lower limit for gimbal position.") + self.keyword_registry.int("softmax", + getter=lambda: self._soft_max, + setter=lambda v: setattr(self, "_soft_max", int(v)), + units=self.units, + description="Software upper limit for gimbal position.") + self.keyword_registry.int("hardmin", + getter=lambda: self._hard_min, + units=self.units, + description="Hardware lower limit for gimbal position.") + self.keyword_registry.int("hardmax", + getter=lambda: self._hard_max, + units=self.units, + description="Hardware upper limit for gimbal position.") + self.keyword_registry.string("status", + getter=self.keyword_wrapper(self.status,key="status"), + description="Grabs status of gimbal mount") + self.keyword_registry.trigger("cleanup", + action=self.clean_up_gimbal, + description="Clean up Gimbal, Open loops and set voltage to 0") + + self.logger.info("Daemon started successfully and connected to hardware") + self.logger.info("Initialized %s", self.daemon_desc) + except ConnectionRefusedError as e: + self.logger.error("Failed to connect to hardware: %s", e) + self.logger.warning("Daemon will start but hardware is not available") + self.state['error'] = str(e) + self.state['connected'] = False + + self.logger.info("Starting %s Daemon", self.daemon_desc) + + def initialize(self): + """handles initialization""" + # for PPC102_Coms, this involves setting the enable + if not self.state['connected']: + return {"ok": False, "error": "Not connected to hardware"} + + try: + self.dev.set_enable(channel = 1, enable = 1) + self.dev.set_enable(channel = 2, enable = 1) + self.state['enabled'] = True + self.logger.debug("Initialized %s", self.daemon_desc) + except Exception as e: # pylint: disable=W0718 + self.logger.error("error: %s",e) + return {"ok":False , "error": str(e)} + return {"ok": True} + + def on_stop(self, libby) -> None: #pylint: disable=W0222 + '''Stops the daemon and disconnects from hardware device''' + try: + self.connect(False) + self.logger.info("Disconnected %s", self.daemon_desc) + except Exception as e: # pylint: disable=W0718 + self.logger.error("Disconnect %s:: Failed ", self.daemon_desc) + self.logger.error("Error: %s",e) + + def connect(self, connect): + """handles connection""" + try: + if connect: + self.dev.connect(host = self.host, port = self.port) + self.logger.info("") + else: + self.dev.disconnect() + result = self.dev.is_connected() + self.logger.info("Connected %s", self.daemon_desc) + except Exception as e: # pylint: disable=W0718 + self.logger.error("Failed to Connect or Disconnect with Hardware: %s",e) + return {"ok": False, "error": str(e)} + return {"ok": True, "isconnected": result} + + def clean_up_gimbal(self): + '''Cleans up gimbal settings''' + try: + self.dev.set_loop(channel=0, loop=1) # Open loops + self.dev.set_enable(channel=0, enable=1) # Device can stay enabled + self.dev.set_output_volts(channel=1, volts=0) # Set output voltages to 0 + self.dev.set_output_volts(channel=2, volts=0) + self.logger.info("Cleaned up %s", self.daemon_desc) + except Exception as e: # pylint: disable=W0718 + self.logger.error("error during cleanup: %s",e) + return {"ok": False, "error": str(e)} + return {"ok": True, "message": "Gimbal cleaned up"} + + def status(self): + """handles status""" + if not self.state['connected']: + return {"ok": False, "error": "Not connected to hardware"} + + try: + res_x = self.dev.get_status_update(channel = 1) + res_y = self.dev.get_status_update(channel = 2) + enabled_x = self.dev.get_enable(channel = 1) + enabled_y = self.dev.get_enable(channel = 2) + enabled = enabled_x == 1 and enabled_y == 1 + status = { + "is_connected": self.dev.is_connected(), + "position_x": res_x[1], + "position_y": res_y[1], + "voltage_x": res_x[0], + "voltage_y": res_y[0], + "flag_x": res_x[2], + "flag_y": res_y[2], + "enabled": enabled, + "isloopsclosed": self.dev.is_loop_closed() + } + self.logger.debug("status: %s",status) + except Exception as e: # pylint: disable=W0718 + self.logger.error("error: %s",e) + return {"ok": False, "error": str(e)} + return {"ok": True, "status": status} + + def is_loops_closed(self): + '''checks if loops are closed''' + if not self.state['connected']: + return {"ok": False, "error": "Not connected to hardware"} + + try: + closed = self.dev.is_loop_closed() + self.logger.debug("is_loops_closed: %s",closed) + except Exception as e: # pylint: disable=W0718 + self.logger.error("error: %s",e) + return {"ok": False, "error": str(e)} + return {"ok":True, "isloopsclosed": closed} + + def set_loops(self, loops: bool): + '''sets control loops''' + try: + if loops: + self.dev.set_loop(channel=0, loop=2) + self.logger.debug("close loops sent") + else: + self.dev.set_loop(channel=0, loop=1) + self.logger.debug("open loops sent") + result = self.dev.is_loop_closed() + if result: + self.logger.debug("loops are closed") + self.units = self.get_config("hardware.closed_loop_units") + self._soft_min = self.get_config("limits.closed_loop.soft_min") + self._soft_max = self.get_config("limits.closed_loop.soft_max") + self._hard_min = self.get_config("limits.closed_loop.hard_min") + self._hard_max = self.get_config("limits.closed_loop.hard_max") + else: + self.logger.debug("loops are open") + self.units = self.get_config("hardware.open_loop_units") + self._soft_min = self.get_config("limits.open_loop.soft_min") + self._soft_max = self.get_config("limits.open_loop.soft_max") + self._hard_min = self.get_config("limits.open_loop.hard_min") + self._hard_max = self.get_config("limits.open_loop.hard_max") + if result != loops: + raise RuntimeError("Failed to execute set loops") + self.state['isloopsclosed'] = result + except Exception as e: # pylint: disable=W0718 + self.logger.error("error: %s",e) + return {"ok": False, "error": str(e)} + return {"ok":True, "isloopsclosed": result} + + def get_xpos(self): + '''gets current X position''' + return self.get_pos(axis=0) + + def get_ypos(self): + '''gets current Y position''' + return self.get_pos(axis=1) + + def get_pos(self, axis : int): + '''gets current position''' + if not self.state['connected']: + return {"ok": False, "error": "Not connected to hardware"} + if not self.state['enabled']: + return {"ok": False, "error": "Device not enabled"} + if not self.state['isloopsclosed']: + return {"ok": False, "error": "Control loops are not closed"} + try: + pos = self.dev.get_pos(channel=axis+1) + position = float(pos) + self.logger.debug("get_pos: %s",position) + except Exception as e: # pylint: disable=W0718 + self.logger.error("error: %s",e) + return {"ok": False, "error": str(e)} + return {"ok":True, "position": position} + + def set_xpos(self, pos: float): + '''sets current X position''' + if self.state['isloopsclosed']: + return self.set_pos(axis=0, pos=pos) + return self.set_volts(axis=0, volts=pos) + + def set_ypos(self, pos: float): + '''sets current Y position''' + if self.state['isloopsclosed']: + return self.set_pos(axis=1, pos=pos) + return self.set_volts(axis=1, volts=pos) + + def set_volts(self, axis: int, volts: float): + '''sets output voltage for open loop control''' + if not self.state['connected']: + return {"ok": False, "error": "Not connected to hardware"} + if not self.state['enabled']: + return {"ok": False, "error": "Device not enabled"} + if self.state['isloopsclosed']: + return {"ok": False, "error": "Control loops are closed; cannot set volts"} + try: + volts = float(volts) + axis = int(axis) + if axis not in [0,1]: + self.logger.error("Axis must be 0 (X) or 1 (Y)") + return {"ok": False, "error": "Axis must be 0 (X) or 1 (Y)"} + chan = axis + 1 + #convert volts to int between -32768 and 32,767 for PPC102 + voltage_ratio = (volts - self.V_MIN) / (self.V_MAX - self.V_MIN) + volts_int = int(voltage_ratio*(self.DIGITAL_MAX-self.DIGITAL_MIN)+self.DIGITAL_MIN) + self.dev.set_output_volts(channel=chan, volts=volts_int) + self.logger.debug("set_volts: %s",volts) + voltage = self.dev.get_status_update(channel=chan)[0] + except Exception as e: # pylint: disable=W0718 + self.logger.error("error: %s",e) + return {"ok": False, "error": str(e)} + return {"ok":True, "voltage": voltage} + + def set_pos(self, axis, pos): + '''sets current position''' + if not self.state['connected']: + return {"ok": False, "error": "Not connected to hardware"} + if not self.state['enabled']: + return {"ok": False, "error": "Device not enabled"} + if not self.state['isloopsclosed']: + return {"ok": False, "error": "Control loops are not closed"} + try: + pos = float(pos) + axis = int(axis) + if axis not in [0,1]: + self.logger.error("Axis must be 0 (X) or 1 (Y)") + return {"ok": False, "error": "Axis must be 0 (X) or 1 (Y)"} + chan = axis + 1 + self.dev.set_pos(channel=chan, pos=pos) + self.logger.debug("set_pos: %s",pos) + position = self.dev.get_pos(channel=chan) + except Exception as e: # pylint: disable=W0718 + self.logger.error("error: %s",e) + return {"ok": False, "error": str(e)} + return {"ok":True, "position": position} + + def goto_named_pos(self, name): + '''moves to named position''' + if not self.state['connected']: + return {"ok": False, "error": "Not connected to hardware"} + + try: + str_goal = name.lower() + goal = self.named_positions.get(str_goal) + if goal is not None: + self.set_pos(axis=0, pos=float(goal[0])) + self.set_pos(axis=1, pos=float(goal[1])) + self.logger.debug("goto_named_pos: %s -> %s",name,goal) + cur_pos = (self.dev.get_pos(channel=1), self.dev.get_pos(channel=2)) + except Exception as e: # pylint: disable=W0718 + self.logger.error("Error: %s",e) + return {"ok": False, "error": str(e)} + return {"ok": True, "named_pos": str_goal, "position": cur_pos} + + def cur_named_position(self): + """Get the name of the current position, if it matches a named position.""" + if not self.state['connected']: + return {"ok": False, "error": "Not connected to hardware"} + + try: + current_pos = ( + float(self.dev.get_pos(channel=1)), + float(self.dev.get_pos(channel=2)) + ) + + for name, pos in self.named_positions.items(): + dx = abs(float(pos[0]) - float(current_pos[0])) + dy = abs(float(pos[1]) - float(current_pos[1])) + if dx <= 0.01 and dy <= 0.01: + return {"ok": True, "named_pos": name, "position": current_pos} + except Exception as e: # pylint: disable=W0718 + self.logger.error("Error: %s",e) + return {"ok": False, "error": str(e)} + return {"ok": True, "named_pos": "unknown", "position": current_pos} #pylint: disable = C0301 + + + def _check_soft_limits(self, pos: int) -> bool: + """Returns True if position is within soft limits.""" + if self._soft_min is not None and pos < self._soft_min: + raise ValueError(f"Position {pos} below soft min {self._soft_min}") + if self._soft_max is not None and pos > self._soft_max: + raise ValueError(f"Position {pos} above soft max {self._soft_max}") + return None + + def _check_named(self, name: str) -> Optional[str]: + if not name: + return "value must be a non-empty string" + if name not in self.named_positions: + return (f"unknown named position '{name}'; " + f"available: {list(self.named_positions)}") + return None + + @staticmethod + def keyword_wrapper(func, key=None): + """Wrap a daemon method for use as a keyword getter/setter.""" + def wrapper(*args, **kwargs): + try: + result = func(*args, **kwargs) + except Exception as e: + print(f"DEBUG keyword_wrapper [{func.__name__}]: exception={e}") + raise + if not result.get("ok"): + raise RuntimeError(result.get("error", f"Unknown error in {func.__name__}")) + if key: + return result[key] + return {k: v for k, v in result.items() if k != "ok"} + return wrapper + + +def main(): + """Main entry point for the daemon.""" + parser = argparse.ArgumentParser( + description='FilterWheel Daemon' + ) + parser.add_argument('-c', '--config', type=str, + help='Path to config file (YAML or JSON)') + parser.add_argument('-d', '--daemon-id', type=str, + help='Daemon ID (required for subsystem configs with multiple daemons)') + + args = parser.parse_args() + + if not args.config: + print("--config is required", file=sys.stderr) + sys.exit(2) + + try: + daemon = PiaaGimbalmount.from_config_file(args.config, daemon_id=args.daemon_id) + daemon.serve() + except KeyboardInterrupt: + print("\nDaemon interrupted by user") + sys.exit(0) + except Exception as e: #pylint: disable=W0718 + print(f"Error running daemon: {e}", file=sys.stderr) + sys.exit(1) + + +if __name__ == '__main__': + main() diff --git a/src/hispec/driver/thorlabs b/src/hispec/driver/thorlabs index 0c39034..89cee42 160000 --- a/src/hispec/driver/thorlabs +++ b/src/hispec/driver/thorlabs @@ -1 +1 @@ -Subproject commit 0c390342f07fe06f0f92469a6a082af28e4a82e1 +Subproject commit 89cee42fb943e88fc17c192e3197396735a904e5