Support 4x encoder resolution mode.

point/gemini.py

@@ -13,6 +13,11 @@
 from typing import TypeVar
 from point.gemini_backend import Gemini2Backend
 from point.gemini_commands import (
+    G2AxisSelect,
+    G2Cmd_ServoQuadratureMode_Get,
+    G2Cmd_ServoQuadratureMode_Set,
+    G2IntPerAxis,
+    G2FloatPerAxis,
     G2Cmd_AlignToObject,
     G2Cmd_DEC_Divisor_Set,
     G2Cmd_DEC_StartStop_Set,
@@ -28,6 +33,7 @@
     G2Cmd_PECReplayOn_Set,
     G2Cmd_PECStatus_Get,
     G2Cmd_PECStatus_Set,
+    G2Cmd_PhysicalAxisPositions_Get,
     G2Cmd_RA_Divisor_Set,
     G2Cmd_RA_StartStop_Set,
     G2Cmd_SelectStartupMode,
@@ -40,6 +46,7 @@
     G2Cmd_SetStoredSite,
     G2Cmd_StartupCheck,
     G2Cmd_SyncToObject,
+    G2Cmd_TicksPerHalfCircle_Get,
     G2Cmd_TogglePrecision,
     G2MacroFields,
     G2PECStatus,
@@ -48,12 +55,24 @@
     G2StartupStatus,
     G2Stopped,
     Gemini2Command,
+    SINT32_MIN,
+    SINT32_MAX,
 )
 from point.gemini_exceptions import Gemini2Exception
 
 
 Gemini2CommandGeneric = TypeVar("Gemini2CommandGeneric", bound=Gemini2Command)
 
+
+# Frequency of the timer used to control the rate of "pulses" sent from the ARM
+# processor to the per-axis PIC motor controllers. Each pulse advances the motor
+# controller's target position forward or backward by a fixed small number of encoder
+# ticks (1 tick in 1x encoder resolution mode or 4 ticks in 4x resolution mode). To
+# achieve a particular slew rate, the timer only sends a pulse once every "divisor"
+# timer periods. Thus the pulse frequency is 12 MHz divided by the divisor value.
+ARM_PULSE_TIMER_FREQ_HZ = 12e6
+
+
 # TODO: Handle UDP response timeouts appropriately
 # TODO: Restore "good" documentation to the classes and functions and stuff
 
@@ -139,6 +158,39 @@ def __init__(
         self._use_multiprocessing = use_multiprocessing
         self.set_double_precision()
 
+        # Degrees of axis rotation per encoder tick for use in conversions between ticks
+        # and degrees. This assumes that the encoder resolution and gear ratios won't
+        # change after construction, which is an okay but not perfect assumption. For
+        # example, if the 4x encoder resolution mode added in Level 6 is changed after
+        # this point then this value will be stale. Also assumes that the encoder
+        # resolution and gear ratios are the same on both axes.
+        ticks_per_half_circle = self.get_ticks_per_half_circle()
+        if ticks_per_half_circle.RA != ticks_per_half_circle.DEC:
+            # This should be uncommon
+            raise Gemini2Exception(
+                f'Unsupported configuration: Different number of encoder ticks per '
+                f'half circle on RA ({ticks_per_half_circle.RA}) versus DEC '
+                f'({ticks_per_half_circle.DEC})'
+            )
+        self.encoder_tick_deg = 180.0 / ticks_per_half_circle.RA
+
+        # Degrees of axis rotation per ARM pulse sent to the motor controller. Each
+        # "pulse" from the ARM processor advances the motor controller's target position
+        # by a fixed amount. Prior to Level 6 firmware, that amount was always equal to
+        # the encoder resolution. In Level 6, the pulse always advances the position by
+        # the encoder step size in 1x resolution mode, even when 4x "quadrature" mode is
+        # enabled.
+        quad_mode = self.get_servo_quadrature_mode()
+        if (G2AxisSelect.RA in quad_mode) != (G2AxisSelect.DEC in quad_mode):
+            raise Gemini2Exception(
+                f'Unsupported configuration: Quadrature servo mode is not the same on '
+                f'both mount axes. Enabled only on {quad_mode.name} axis.'
+            )
+        if quad_mode == G2AxisSelect.RA | G2AxisSelect.DEC:
+            self.arm_pulse_deg = 4 * self.encoder_tick_deg
+        else:
+            self.arm_pulse_deg = self.encoder_tick_deg
+
         if use_multiprocessing:
             self._slew_rate_processes = {}
             self._slew_rate_target = {}
@@ -526,6 +578,24 @@ def get_stored_site(self) -> int:
 
     ### Native Commands
 
+    def get_ticks_per_half_circle(self) -> G2IntPerAxis:
+        """Encoder ticks per half circle of axis rotation for RA and Dec."""
+        return self.exec_cmd(G2Cmd_TicksPerHalfCircle_Get()).ticks_per_half_circle
+
+    def get_physical_axis_positions(self) -> G2FloatPerAxis:
+        """Get the physical axis positions in degrees."""
+        cmd = self.exec_cmd(G2Cmd_PhysicalAxisPositions_Get())
+        return G2FloatPerAxis(
+            self.encoder_tick_deg * cmd.positions.RA,
+            self.encoder_tick_deg * cmd.positions.DEC,
+        )
+
+    def set_servo_quadrature_mode(self, enable: G2AxisSelect) -> None:
+        self.exec_cmd(G2Cmd_ServoQuadratureMode_Set(enable))
+
+    def get_servo_quadrature_mode(self) -> G2AxisSelect:
+        return self.exec_cmd(G2Cmd_ServoQuadratureMode_Get()).enabled
+
     def set_pec_boot_playback(self, enable: bool) -> None:
         self.exec_cmd(G2Cmd_PECBootPlayback_Set(enable))
 
@@ -940,9 +1010,15 @@ def slew_rate_to_div(self, rate: float) -> int:
             Divisor setting that is as close to the desired slew rate as possible.
         """
         if rate == 0.0:
+            # Special case and avoids division by zero.
             return 0
-        # TODO: Replace hard-coded constants with values read from Gemini in constructor
-        return int(12e6 / (6400.0 * rate))
+
+        div = round(ARM_PULSE_TIMER_FREQ_HZ * self.arm_pulse_deg / rate)
+        if div < SINT32_MIN:
+            div = SINT32_MIN
+        elif div > SINT32_MAX:
+            div = SINT32_MAX
+        return div
 
     def div_to_slew_rate(self, div: int) -> float:
         """Convert a divisor setting to corresponding slew rate.
@@ -955,8 +1031,7 @@ def div_to_slew_rate(self, div: int) -> float:
         """
         if div == 0:
             return 0.0
-        # TODO: Replace hard-coded constants with values read from Gemini in constructor
-        return 12e6 / (6400.0 * div)
+        return ARM_PULSE_TIMER_FREQ_HZ * self.arm_pulse_deg / div
 
     def stop_motion(self) -> None:
         """Stops motion on both axes.

point/gemini_commands.py

@@ -459,6 +459,21 @@ class Gemini2Command_Native_Set(Gemini2Command_Native):
 
 ### Enumerations, Constants, etc
 
+@dataclass(frozen=True)
+class G2BoolPerAxis:
+    RA: bool
+    DEC: bool
+
+@dataclass(frozen=True)
+class G2IntPerAxis:
+    RA: int
+    DEC: int
+
+@dataclass(frozen=True)
+class G2FloatPerAxis:
+    RA: float
+    DEC: float
+
 
 class G2Precision(Enum):
     """Parameter for GetPrecision."""
@@ -541,6 +556,12 @@ class G2Status(Flag):
     ASSUMING_J2000_OBJ_COORDS = 1 << 5
 
 
+class G2AxisSelect(Flag):
+    # Values are specific to native commands 401, 402, and 403.
+    RA = 1 << 0
+    DEC = 1 << 1
+
+
 # Parameter for MacroENQ fields 'servo_duty_x' and 'servo_duty_y'.
 G2_SERVO_DUTY_MIN = -100
 G2_SERVO_DUTY_MAX = 100
@@ -971,6 +992,50 @@ def interpret(self) -> None:
 #    def response(self):      return None # TODO!
 
 
+class G2Cmd_TicksPerHalfCircle_Get(Gemini2Command_Native_Get):
+    response_expected = True
+    native_id = 238
+    ticks_per_half_circle: G2IntPerAxis
+
+    def interpret(self):
+        match = re.match(r'(\d+);(\d+)', self.raw_response)
+        if match is None:
+            raise G2ResponseInterpretationFailure(
+                f'Could not parse {self.raw_response}'
+            )
+        self.ticks_per_half_circle = G2IntPerAxis(int(match[1]), int(match[2]))
+
+
+class G2Cmd_PhysicalAxisPositions_Get(Gemini2Command_Native_Get):
+    response_expected = True
+    native_id = 239
+    positions: G2IntPerAxis
+
+    def interpret(self):
+        match = re.match(r'(\d+);(\d+)', self.raw_response)
+        if match is None:
+            raise G2ResponseInterpretationFailure(
+                f'Could not parse {self.raw_response}'
+            )
+        self.positions = G2IntPerAxis(int(match[1]), int(match[2]))
+
+
+class G2Cmd_ServoQuadratureMode_Set(Gemini2Command_Native_Set):
+    native_id = 401
+
+    def __init__(self, enable: G2AxisSelect):
+        self.native_params = (str(enable.value),)
+
+
+class G2Cmd_ServoQuadratureMode_Get(Gemini2Command_Native_Get):
+    response_expected = True
+    native_id = 401
+    enabled: G2AxisSelect
+
+    def interpret(self):
+        self.enabled = G2AxisSelect(parse_int(self.raw_response))
+
+
 class G2Cmd_PECBootPlayback_Set(Gemini2Command_Native_Set):
     native_id = 508
 
@@ -1043,11 +1108,8 @@ class G2CmdBase_Divisor_Set(Gemini2Command_Native_Set):
     def __init__(self, div: int):
         if not isinstance(div, int):
             raise G2CommandParameterTypeError('int')
-        # clamp divisor into the allowable range
-        if div < SINT32_MIN:
-            div = SINT32_MIN
-        if div > SINT32_MAX:
-            div = SINT32_MAX
+        if not SINT32_MIN <= div <= SINT32_MAX:
+            raise G2CommandParameterValueError(f'{div=} is not a 32-bit integer.')
         self.native_params = (str(div),)