_helpers.py

""" Helper functions for RapidCode Python samples.
"""
 
import struct
import sys
 
from _imports import RapidCode, RAPIDCODE_DIR, constants, platform
 
 
# ---------------------------------------------------------------------------
# stdin_is_interactive / key_pressed(): non-blocking "has the user pressed a key?"
# for sample loops.
#
# The platform decision is made ONCE at import time by binding key_pressed to the
# right private implementation, so the per-call cost is a single function call and
# the platform-specific imports (msvcrt / select) only happen where they exist.
# When stdin is not an interactive console (CI runs the samples with stdin
# redirected), stdin_is_interactive is False and key_pressed is bound to a stub that
# always returns False, so a sample polling it can never block or throw in an
# automated run. Samples should also stop on their own when not interactive.
# ---------------------------------------------------------------------------
 
def _stdin_is_console_windows() -> bool:
    """True only for a real console: isatty() is also True for the NUL device on Windows."""
    import ctypes
    import msvcrt
    mode = ctypes.c_uint()
    handle = msvcrt.get_osfhandle(sys.stdin.fileno())
    return bool(ctypes.windll.kernel32.GetConsoleMode(handle, ctypes.byref(mode)))
 
 
def _key_pressed_no_terminal() -> bool:
    """stdin is not a terminal (automated test run): never reports a key press."""
    return False
 
 
def _key_pressed_windows() -> bool:
    """Windows console: msvcrt.kbhit() reports a pending key press without blocking."""
    return _msvcrt.kbhit()
 
 
def _key_pressed_posix() -> bool:
    """Linux/macOS terminal: select() with a zero timeout polls stdin without blocking.
    Note the terminal is line-buffered by default, so this reports the key after Enter."""
    readable, _, _ = _select.select([sys.stdin], [], [], 0)
    return bool(readable)
 
 
try:
    stdin_is_interactive = sys.stdin.isatty() and (platform.system() != "Windows" or _stdin_is_console_windows())
except (AttributeError, OSError, ValueError):  # stdin closed or replaced
    stdin_is_interactive = False
 
if not stdin_is_interactive:
    key_pressed = _key_pressed_no_terminal
elif platform.system() == "Windows":
    import msvcrt as _msvcrt
    key_pressed = _key_pressed_windows
else:
    import select as _select
    key_pressed = _key_pressed_posix
 
 
# EtherCAT AL Status register (0x0130): bits 0-3 are the state, bit 4 is the error flag.
AL_STATUS_STATES = {
    0x01: "INIT",
    0x02: "PREOP",
    0x03: "BOOT",
    0x04: "SAFEOP",
    0x08: "OP",
}
AL_STATUS_ERROR_SUFFIX = "+ERR"
 
 
def decode_al_status(al_status: int) -> str:
    """Decode one node's AL Status register to its state name, e.g. OP or SAFEOP+ERR."""
    state_name = AL_STATUS_STATES.get(al_status & 0x0F, "?")
    if al_status & 0x10:
        state_name += AL_STATUS_ERROR_SUFFIX
    return state_name
 
 
def decode_al_status_ored(al_status: int) -> str:
    """Decode the network-wide AL Status (NetworkStatus.AlStatus), the OR of every node's
    register, to the states present, e.g. OP or SAFEOP|OP+ERR.
 
    Only the single-bit states are unambiguous when ORed: BOOT (3) is INIT (1) | PREOP (2),
    so a network showing INIT|PREOP may hold a node in BOOT."""
    states_present = [name for bit, name in ((0x01, "INIT"), (0x02, "PREOP"), (0x04, "SAFEOP"), (0x08, "OP"))
                      if al_status & bit]
    state_name = "|".join(states_present) if states_present else "NONE"
    if al_status & 0x10:
        state_name += AL_STATUS_ERROR_SUFFIX
    return state_name
 
 
def get_enum_name(prefix: str, value: int) -> str:
    """Get enum name using reflection on SWIG-generated constants.
 
    Args:
        prefix: The enum prefix (e.g., "RSINetworkState_RSINetworkState")
        value: The enum value to look up
 
    Returns:
        The enum name with prefix stripped, or "UNKNOWN(value)" if not found
 
    Example:
        get_enum_name("RSINetworkState_RSINetworkState", controller.NetworkStateGet())
        # Returns "OPERATIONAL" for RSINetworkState_RSINetworkStateOPERATIONAL
    """
    for name in dir(RapidCode):
        if name.startswith(prefix):
            if getattr(RapidCode, name) == value:
                return name[len(prefix):]
    return f"UNKNOWN({value})"
 
 
def get_creation_parameters():
  # create a motion controller and return it. 
  # If any errors are found, raise an exception with the error log as the message.
  creation_params: RapidCode.CreationParameters = RapidCode.CreationParameters()
  creation_params.RmpPath = RAPIDCODE_DIR
  creation_params.NicPrimary = constants.RMP_NIC_PRIMARY
 
  if platform.system() == "Windows":
    creation_params.NodeName = constants.RMP_NODE_NAME
  elif platform.system() == "Linux":
    creation_params.CpuAffinity = constants.RMP_CPU_AFFINITY
  else:
    raise Exception("Unsupported platform")
 
  return creation_params
 
def check_errors(rsi_object):
  # check for errors in the given rsi_object and print any errors that are found. 
  # If the error log contains any errors (not just warnings), raises an exception with the error log as the message.
  # returns a tuple containing a boolean indicating whether the error log contained any errors and the error log string.
  error_string_builder = ""
  i = rsi_object.ErrorLogCountGet()
  while rsi_object.ErrorLogCountGet() > 0:
      error:RapidCode.RsiError = rsi_object.ErrorLogGet()
      error_type = "WARNING" if error.isWarning else "ERROR"
      error_string_builder += f"{error_type}: {error.text}\n"
      if len(error_string_builder) > 0:
          print(error_string_builder)
      if "ERROR" in error_string_builder:
          raise Exception(error_string_builder)
  return "ERROR" in error_string_builder, error_string_builder
 
def start_the_network(controller):
  # attempts to start the network using the given MotionController object. 
  # If the network fails to start, it reads and prints any log messages that may be helpful 
  # in determining the cause of the problem, and then raises an RsiError exception.
  if controller.NetworkStateGet() != RapidCode.RSINetworkState_RSINetworkStateOPERATIONAL:  # Check if network is started already.
    print("Starting Network..")
    controller.NetworkStart()                                                               # If not. Initialize The Network. (This can also be done from RapidSetup Tool)
 
  if controller.NetworkStateGet() != RapidCode.RSINetworkState_RSINetworkStateOPERATIONAL:  # Check if network is started again.
    start_error = controller.LastNetworkStartErrorGet()
    print(
      "Network start error: "
      f"{enum_to_name(start_error, 'RSINetworkStartError')} ({start_error})"
    )
 
    messages_to_read = controller.NetworkLogMessageCountGet()                               # Some kind of error starting the network, read the network log messages
 
    for i in range(messages_to_read):
      print(controller.NetworkLogMessageGet(i))                                             # Print all the messages to help figure out the problem
    print("Expected OPERATIONAL state but the network did not get there.")
    #  raise Exception(Expected OPERATIONAL state but the network did not get there.)       # Uncomment if you want your application to exit when the network isn't operational. (Comment when using phantom axis)
  else:                                                                                     # Else, of network is operational.
    print("Network Started")
 
def abort_motion_object(motion_object):
  # Aborts motion on the given motion object (Axis or MultiAxis), waits for motion to complete,
  # clears faults, and verifies the object enters IDLE state.
  # If the object fails to enter IDLE state, raises an exception with the error source.
  motion_object.EStopAbort()
  motion_object.MotionDoneWait()
  motion_object.ClearFaults()
 
  # check for idle state
  verify_idle_state(motion_object)
 
def verify_idle_state(motion_object):
  # Verifies that the given motion object (Axis or MultiAxis) is in IDLE state.
  # If not, raises an exception with the error source.
  if motion_object.StateGet() != RapidCode.RSIState_RSIStateIDLE:
    source = motion_object.SourceGet()  # get state source enum
    error_msg = f"Axis or MultiAxis {motion_object.NumberGet()} is expected to be in IDLE state, but is in state {enum_to_name(motion_object.StateGet(), 'RSIState')}. " \
                f"\nError Source: {motion_object.SourceNameGet(source)}"
    raise Exception(error_msg)
 
def enum_to_name(value, prefix):
    """Reverse lookup: int value -> enum name"""
    for name in dir(RapidCode):
        if name.startswith(prefix + "_" + prefix) and getattr(RapidCode, name) == value:
            return name.split(prefix + "_" + prefix)[1]
    return str(value)
 
# RSIDataType name -> FirmwareValue attribute holding a value of that type. Every RSIDataType
# member is covered (masks are read as their unsigned width; SHORT/USHORT are the deprecated
# spellings of INT16/UINT16).
_FIRMWARE_VALUE_ATTRIBUTES = {
    "BOOL": "Bool",
    "INT8": "Int8", "UINT8": "UInt8",
    "INT16": "Int16", "UINT16": "UInt16", "SHORT": "Int16", "USHORT": "UInt16",
    "INT32": "Int32", "UINT32": "UInt32", "MASK32": "UInt32",
    "INT64": "Int64", "UINT64": "UInt64", "MASK64": "UInt64",
    "FLOAT": "Float", "DOUBLE": "Double",
}
 
 
def firmware_value_attribute(data_type) -> str:
    """Name of the FirmwareValue attribute that holds a value of the given RSIDataType.
    Look it up once per address (it reflects over the RapidCode module), then read many
    values with getattr(firmware_value, attribute)."""
    data_type_name = enum_to_name(data_type, "RSIDataType")
    try:
        return _FIRMWARE_VALUE_ATTRIBUTES[data_type_name]
    except KeyError:
        raise ValueError(f"RSIDataType {data_type_name} cannot be read from a FirmwareValue") from None
 
 
def recorder_value_reader(controller, data_type):
    """The cheapest way to read one recorded value of the given RSIDataType from Python.
 
    Returns a callable reader(recorder_number, record_index, data_index) for use after
    RecorderRecordDataRetrieveBulk. Each RecorderRecordData*Get call crosses the SWIG
    boundary once, and that crossing dominates the cost of draining a Recorder from Python:
    RecorderRecordDataValueGet / RecorderRecordDataDoubleGet return a plain number (about
    0.65 us per value), while RecorderRecordDataFirmwareValueGet returns a FirmwareValue
    object that then needs an attribute read (about 1.3 us). A large Recorder set (many nodes,
    every sample) only keeps up with the plain getters.
 
    The Recorder copies 8 bytes from every address. Types up to 32 bits use ValueGet (the
    low 32 bits) and are masked to their width and, if signed, sign-extended, so the bytes
    next to a narrow value do not leak in; FLOAT reinterprets those 32 bits. DOUBLE uses
    DoubleGet. 64-bit integer types use DoubleGet and reinterpret the 8 bytes, which is exact
    unless the value's top 12 bits are all set (a NaN bit pattern): counters never get there.
    """
    data_type_name = enum_to_name(data_type, "RSIDataType")
    value_get = controller.RecorderRecordDataValueGet
    double_get = controller.RecorderRecordDataDoubleGet
    if data_type_name == "INT32":
        return value_get
    if data_type_name in ("UINT32", "MASK32"):
        return lambda recorder, record, index: value_get(recorder, record, index) & 0xFFFFFFFF
    if data_type_name == "BOOL":
        return lambda recorder, record, index: bool(value_get(recorder, record, index) & 0xFF)
    unsigned_widths = {"UINT8": 8, "UINT16": 16, "USHORT": 16}
    if data_type_name in unsigned_widths:
        mask = (1 << unsigned_widths[data_type_name]) - 1
        return lambda recorder, record, index: value_get(recorder, record, index) & mask
    signed_widths = {"INT8": 8, "INT16": 16, "SHORT": 16}
    if data_type_name in signed_widths:
        mask = (1 << signed_widths[data_type_name]) - 1
        sign_bit = 1 << (signed_widths[data_type_name] - 1)
        return lambda recorder, record, index: ((value_get(recorder, record, index) & mask) ^ sign_bit) - sign_bit
    if data_type_name == "FLOAT":
        return lambda recorder, record, index: struct.unpack("<f", struct.pack("<I", value_get(recorder, record, index) & 0xFFFFFFFF))[0]
    if data_type_name == "DOUBLE":
        return double_get
    if data_type_name in ("UINT64", "MASK64"):
        return lambda recorder, record, index: struct.unpack("<Q", struct.pack("<d", double_get(recorder, record, index)))[0]
    if data_type_name == "INT64":
        return lambda recorder, record, index: struct.unpack("<q", struct.pack("<d", double_get(recorder, record, index)))[0]
    raise ValueError(f"RSIDataType {data_type_name} cannot be read from a Recorder record")
 
 
def firmware_value_get(firmware_value, data_type):
    """Read a FirmwareValue (a 64-bit union) as the Python value of the given RSIDataType.
 
    Pair it with the AddressDataTypeGet family (Axis, MultiAxis, MotionController,
    RapidCodeNetworkNode) so recorded or interrupt user data is interpreted with the
    type the firmware actually stores at that address, e.g.:
 
        data_type = node.AddressDataTypeGet(RapidCode.RSINetworkNodeAddressType_RSINetworkNodeAddressTypeAL_STATUS)
        value = helpers.firmware_value_get(controller.RecorderRecordDataFirmwareValueGet(0), data_type)
    """
    return getattr(firmware_value, firmware_value_attribute(data_type))