csaxs_bec/devices/epics/flighttube.py

@@ -0,0 +1,291 @@
+"""Example pseudo positioners for a flight tube."""
+
+from __future__ import annotations
+
+from typing import TYPE_CHECKING, Literal
+
+import numpy as np
+from ophyd import Component as Cpt
+from ophyd import Kind, Signal
+from ophyd_devices.interfaces.base_classes.psi_pseudo_motor_base import PSIPseudoMotorBase
+
+if TYPE_CHECKING:
+    from bec_lib.devicemanager import DeviceManagerBase
+
+
+class _FlightTubeBase(PSIPseudoMotorBase):
+    """Shared geometry helpers for the flight-tube examples."""
+
+    def __init__(
+        self,
+        name: str,
+        front_motor: str,
+        rear_motor: str,
+        front_overhang: float,
+        rear_overhang: float,
+        support_span: float,
+        device_manager: DeviceManagerBase,
+        egu: str,
+        **kwargs,
+    ) -> None:
+        """Initialize the shared flight-tube geometry.
+
+        Args:
+            name (str): Device name.
+            front_motor (str): Name of the front height motor.
+            rear_motor (str): Name of the rear height motor.
+            front_overhang (float): Distance from the front support to the tube front in
+                motor units.
+            rear_overhang (float): Distance from the rear support to the tube rear in
+                motor units.
+            support_span (float): Distance between the front and rear supports in motor
+                units.
+            device_manager (DeviceManagerBase): BEC device manager instance.
+            egu (str): Engineering unit exposed by the pseudo motor.
+            **kwargs: Additional keyword arguments forwarded to the base class.
+        """
+        positioners = self.get_positioner_objects(
+            name=name,
+            positioners={"front": front_motor, "rear": rear_motor},
+            device_manager=device_manager,
+        )
+        super().__init__(
+            name=name, device_manager=device_manager, positioners=positioners, egu=egu, **kwargs
+        )
+
+        self._front_overhang = front_overhang
+        self._rear_overhang = rear_overhang
+        self._support_span = support_span
+        if self._support_span <= 0:
+            raise ValueError("The support span must be greater than zero.")
+
+    def _tube_coordinate(self, reference: Literal["front", "rear", "center"]) -> float:
+        """Return a coordinate along the tube measured from the front support.
+
+        Args:
+            reference (Literal["front", "rear", "center"]): Reference point on the tube.
+
+        Returns:
+            float: Coordinate in motor units.
+        """
+
+        match reference:
+            case "front":
+                return -self._front_overhang
+            case "rear":
+                return self._support_span + self._rear_overhang
+            case "center":
+                return (-self._front_overhang + self._support_span + self._rear_overhang) / 2.0
+
+        raise ValueError(
+            f"Unsupported reference point {reference}. Use 'front', 'rear', or 'center'."
+        )
+
+    def _height_at_coordinate(self, front_height: float, rear_height: float, x: float) -> float:
+        """Return the height at an arbitrary tube coordinate.
+
+        Args:
+            front_height (float): Front support height in motor units.
+            rear_height (float): Rear support height in motor units.
+            x (float): Tube coordinate measured from the front support in motor units.
+
+        Returns:
+            float: Height in motor units.
+        """
+        slope = (rear_height - front_height) / self._support_span
+        return float(front_height + slope * x)
+
+    def _angle_deg(self, front_height: float, rear_height: float) -> float:
+        """Return the tube pitch angle in degrees.
+
+        Args:
+            front_height (float): Front support height in motor units.
+            rear_height (float): Rear support height in motor units.
+
+        Returns:
+            float: Pitch angle in degrees.
+        """
+        delta_height = rear_height - front_height
+        return float(np.rad2deg(np.arctan2(delta_height, self._support_span)))
+
+    def motors_are_moving(self, front: Signal, rear: Signal) -> int:
+        return int(front.get() or rear.get())
+
+
+class FlightTubeHeight(_FlightTubeBase):
+    """Expose the height of a reference point while keeping tube angle fixed.
+
+    The front and rear real motors are expected to use the same engineering
+    unit. The tube geometry is defined in that same unit system.
+    ``zero_height_offset`` calibrates which reference-point height should be
+    reported as ``0``.
+    """
+
+    def __init__(
+        self,
+        name: str,
+        front_motor: str,
+        rear_motor: str,
+        front_overhang: float,
+        rear_overhang: float,
+        support_span: float,
+        device_manager: DeviceManagerBase,
+        height_reference: Literal["front", "rear", "center"] = "front",
+        zero_height_offset: float = 0.0,
+        **kwargs,
+    ) -> None:
+        """Initialize the height pseudo positioner.
+
+        Args:
+            name (str): Device name.
+            front_motor (str): Name of the front height motor.
+            rear_motor (str): Name of the rear height motor.
+            front_overhang (float): Distance from the front support to the tube front in
+                motor units.
+            rear_overhang (float): Distance from the rear support to the tube rear in
+                motor units.
+            support_span (float): Distance between the supports in motor units.
+            device_manager (DeviceManagerBase): BEC device manager instance.
+            height_reference (Literal["front", "rear", "center"]): Tube
+                reference point whose height is exposed.
+            zero_height_offset (float): Reference-point height, in motor units,
+                that should be interpreted as ``0``.
+            **kwargs: Additional keyword arguments forwarded to the base class.
+        """
+        super().__init__(
+            name=name,
+            front_motor=front_motor,
+            rear_motor=rear_motor,
+            front_overhang=front_overhang,
+            rear_overhang=rear_overhang,
+            support_span=support_span,
+            device_manager=device_manager,
+            egu="motor units",
+            **kwargs,
+        )
+        self._height_reference = height_reference
+        self._zero_height_offset = zero_height_offset
+
+    def forward_calculation(self, front: Signal, rear: Signal) -> float:
+        """Convert real motor positions to the reference-point height.
+
+        Args:
+            front (Signal): Front motor signal.
+            rear (Signal): Rear motor signal.
+
+        Returns:
+            float: Reference-point height in motor units.
+        """
+        x_ref = self._tube_coordinate(self._height_reference)
+        return self._height_at_coordinate(front.get(), rear.get(), x_ref) - self._zero_height_offset
+
+    def inverse_calculation(self, position: float, front: Signal, rear: Signal) -> dict[str, float]:
+        """Translate the selected reference point while keeping the angle fixed.
+
+        Args:
+            position (float): Target reference-point height in motor units.
+            front (Signal): Front motor signal.
+            rear (Signal): Rear motor signal.
+
+        Returns:
+            dict[str, float]: Target front and rear motor positions.
+        """
+        current_front = float(front.get())
+        current_rear = float(rear.get())
+        x_ref = self._tube_coordinate(self._height_reference)
+        current_height = self._height_at_coordinate(current_front, current_rear, x_ref)
+        target_height = position + self._zero_height_offset
+        delta = target_height - current_height
+        return {"front": current_front + delta, "rear": current_rear + delta}
+
+
+class FlightTubeAngle(_FlightTubeBase):
+    """Expose the flight-tube pitch angle.
+
+    The front and rear real motors are expected to use the same engineering
+    unit. The tube geometry is defined in that same unit system.
+
+    Attributes:
+        rotation_anchor: Reference point used as the rotation anchor. Valid
+            values are ``"front"``, ``"rear"``, and ``"center"``.
+    """
+
+    rotation_anchor = Cpt(Signal, value="center", kind=Kind.config)
+
+    def __init__(
+        self,
+        name: str,
+        front_motor: str,
+        rear_motor: str,
+        front_overhang: float,
+        rear_overhang: float,
+        support_span: float,
+        device_manager: DeviceManagerBase,
+        zero_angle_height_offset: float = 0.0,
+        **kwargs,
+    ) -> None:
+        """Initialize the angle pseudo positioner.
+
+        Args:
+            name (str): Device name.
+            front_motor (str): Name of the front height motor.
+            rear_motor (str): Name of the rear height motor.
+            front_overhang (float): Distance from the front support to the tube front in
+                motor units.
+            rear_overhang (float): Distance from the rear support to the tube rear in
+                motor units.
+            support_span (float): Distance between the supports in motor units.
+            device_manager (DeviceManagerBase): BEC device manager instance.
+            zero_angle_height_offset (float): Rear-minus-front height
+                difference, in motor units, that should be interpreted as
+                ``0 deg``.
+            **kwargs: Additional keyword arguments forwarded to the base class.
+        """
+        super().__init__(
+            name=name,
+            front_motor=front_motor,
+            rear_motor=rear_motor,
+            front_overhang=front_overhang,
+            rear_overhang=rear_overhang,
+            support_span=support_span,
+            device_manager=device_manager,
+            egu="deg",
+            **kwargs,
+        )
+        self._zero_angle_height_offset = zero_angle_height_offset
+
+    def forward_calculation(self, front: Signal, rear: Signal) -> float:
+        """Convert real motor positions to the calibrated tube angle.
+
+        Args:
+            front (Signal): Front motor signal.
+            rear (Signal): Rear motor signal.
+
+        Returns:
+            float: Tube angle in degrees.
+        """
+        return self._angle_deg(front.get(), rear.get() - self._zero_angle_height_offset)
+
+    def inverse_calculation(self, position: float, front: Signal, rear: Signal) -> dict[str, float]:
+        """Rotate the tube around the selected anchor.
+
+        Args:
+            position (float): Target tube angle in degrees.
+            front (Signal): Front motor signal.
+            rear (Signal): Rear motor signal.
+
+        Returns:
+            dict[str, float]: Target front and rear motor positions.
+        """
+        current_front = float(front.get())
+        current_rear = float(rear.get())
+        target_delta_height = (
+            self._zero_angle_height_offset + np.tan(np.deg2rad(position)) * self._support_span
+        )
+        slope = target_delta_height / self._support_span
+        anchor = self.rotation_anchor.get()
+        x_anchor = self._tube_coordinate(anchor)
+        anchor_height = self._height_at_coordinate(current_front, current_rear, x_anchor)
+        new_front = anchor_height - slope * x_anchor
+        new_rear = anchor_height + slope * (self._support_span - x_anchor)
+        return {"front": float(new_front), "rear": float(new_rear)}