Minimal hal for board tests

Author: WillB97

kit_test/hal/__init__.py

@@ -0,0 +1,20 @@
+from .discovery import VidPid, discover_boards
+from .motor_board import VIDPID as MOTOR_VIDPID
+from .motor_board import MotorBoard
+from .power_board import BRAIN_OUTPUT, PowerBoard, PowerOutputPosition
+from .power_board import VIDPID as POWER_VIDPID
+from .servo_board import VIDPID as SERVO_VIDPID
+from .servo_board import ServoBoard
+
+__all__ = [
+    'BRAIN_OUTPUT',
+    'MOTOR_VIDPID',
+    'POWER_VIDPID',
+    'SERVO_VIDPID',
+    'MotorBoard',
+    'PowerBoard',
+    'PowerOutputPosition',
+    'ServoBoard',
+    'VidPid',
+    'discover_boards',
+]

kit_test/hal/discovery.py

@@ -0,0 +1,87 @@
+"""Board discovery helper methods."""
+from __future__ import annotations
+
+import logging
+from typing import NamedTuple
+
+from serial.tools.list_ports import comports
+from serial.tools.list_ports_common import ListPortInfo
+
+from .utils import BoardIdentity
+
+logger = logging.getLogger(__name__)
+
+
+class VidPid(NamedTuple):
+    """A named tuple containing the vendor ID and product ID of a USB device."""
+
+    vendor_id: int
+    product_id: int
+
+    def __str__(self) -> str:
+        return f"{self.vendor_id:04X}:{self.product_id:04X}"
+
+
+class Port(NamedTuple):
+    """A named tuple containing the port name and USB identity."""
+
+    port: str
+    identity: BoardIdentity
+
+    def __str__(self) -> str:
+        return f"{self.port} ({self.identity})"
+
+
+def discover_boards(pidvids: list[VidPid] | VidPid) -> list[Port]:
+    """
+    Discover boards connected to the system.
+
+    :param pidvids: A list of vendor ID and product ID pairs to search for.
+        If a single pair is given, it will be used as a filter.
+    :return: A list of ports for the discovered boards.
+    """
+    if isinstance(pidvids, VidPid):
+        pidvids = [pidvids]
+
+    boards: list[Port] = []
+
+    for port in comports():
+        if port.vid is None or port.pid is None:
+            # Skip ports that don't have a VID or PID
+            continue
+
+        vidpid = VidPid(port.vid, port.pid)
+        # Filter to USB vendor and product ID values provided
+        if vidpid not in pidvids:
+            continue
+
+        # Create board identity from USB port info
+        initial_identity = get_USB_identity(port)
+
+        boards.append(
+            Port(port.device, initial_identity)
+        )
+
+    return boards
+
+
+def get_USB_identity(port: ListPortInfo) -> BoardIdentity:
+    """
+    Generate an approximate identity for a board using the USB descriptor.
+
+    This data will be overridden by the firmware once communication is established,
+    but is used for early logging messages and error handling.
+
+    :param port: The USB port information from pyserial
+    :return: An initial identity for the board
+    """
+    try:
+        return BoardIdentity(
+            manufacturer=port.manufacturer or "",
+            board_type=port.product or "",
+            asset_tag=port.serial_number or "",
+        )
+    except Exception:
+        logger.warning(
+            f"Failed to pull identifying information from serial device {port.device}")
+        return BoardIdentity()

kit_test/hal/motor_board.py

@@ -0,0 +1,194 @@
+"""The motor board module provides an interface to the motor board firmware over serial."""
+from __future__ import annotations
+
+import atexit
+import logging
+from enum import IntEnum
+from typing import NamedTuple
+
+from .discovery import VidPid
+from .serial_wrapper import SerialWrapper
+from .utils import (
+    BoardIdentity,
+    map_to_float,
+    map_to_int,
+)
+
+logger = logging.getLogger(__name__)
+BAUDRATE = 115200
+VIDPID = VidPid(0x0403, 0x6001)  # FTDI FT232R chip used on the motor board
+
+
+class MotorPower(IntEnum):
+    """Special values for motor power."""
+
+    BRAKE = 0
+    COAST = -1024  # A value outside the allowable range
+
+
+class MotorStatus(NamedTuple):
+    """A tuple representing the status of the motor board."""
+
+    output_faults: tuple[bool, ...]
+    input_voltage: float
+    other: tuple[str, ...] = tuple()
+
+    @classmethod
+    def from_status_response(cls, response: str) -> MotorStatus:
+        """
+        Create a MotorStatus object from the response to a status command.
+
+        :param response: The response from a *STATUS? command.
+        :raise TypeError: If the response is invalid.
+        :return: A MotorStatus object.
+        """
+        output_fault_str, input_voltage_mv, *other = response.split(':')
+        output_faults = tuple((port == '1') for port in output_fault_str.split(','))
+        input_voltage = float(input_voltage_mv) / 1000
+        return cls(output_faults, input_voltage, tuple(other))
+
+
+class MotorBoard:
+    """
+    A class representing the motor board interface.
+
+    This class is intended to be used to communicate with the motor board over serial
+    using the text-based protocol added in version 4.4 of the motor board firmware.
+
+    :param serial_port: The serial port to connect to.
+    :param initial_identity: The identity of the board, as reported by the USB descriptor.
+    """
+
+    def __init__(
+        self,
+        serial_port: str,
+        initial_identity: BoardIdentity | None = None,
+    ) -> None:
+        if initial_identity is None:
+            initial_identity = BoardIdentity()
+        self._serial = SerialWrapper(serial_port, BAUDRATE, identity=initial_identity)
+
+        self.motors = (
+            Motor(self._serial, 0),
+            Motor(self._serial, 1)
+        )
+
+        identity = self.identify()
+        assert identity.board_type == 'MCv4B', \
+            f"Expected board type 'MCv4B', got {identity.board_type!r} instead."
+
+        self._serial.set_identity(identity)
+
+        # Disable motors on exit
+        atexit.register(self._cleanup)
+
+    def identify(self) -> BoardIdentity:
+        """
+        Get the identity of the board.
+
+        :return: The identity of the board.
+        """
+        response = self._serial.query('*IDN?')
+        return BoardIdentity(*response.split(':'))
+
+    def status(self) -> MotorStatus:
+        """
+        The status of the board.
+
+        :return: The status of the board.
+        """
+        response = self._serial.query('*STATUS?')
+        return MotorStatus.from_status_response(response)
+
+    def reset(self) -> None:
+        """
+        Reset the board.
+
+        This command disables the motors and clears all faults.
+        """
+        self._serial.write('*RESET')
+
+    def _cleanup(self) -> None:
+        """
+        Disable the motors while exiting.
+
+        This method is registered as an exit handler.
+        """
+        try:
+            self.reset()
+        except Exception:
+            logger.warning(f"Failed to cleanup motor board {self._serial}.")
+
+    def __repr__(self) -> str:
+        return f"<{self.__class__.__qualname__}: {self._serial}>"
+
+
+class Motor:
+    """
+    A class representing a motor on the motor board.
+
+    Each motor is controlled through the power property
+    and its current can be read using the current property.
+
+    :param serial: The serial wrapper to use to communicate with the board.
+    :param index: The index of the motor on the board.
+    """
+
+    def __init__(self, serial: SerialWrapper, index: int):
+        self._serial = serial
+        self._index = index
+
+    def get_power(self) -> float:
+        """
+        Read the current power setting of the motor.
+
+        :return: The power of the motor as a float between -1.0 and 1.0
+            or the special value MotorPower.COAST.
+        """
+        response = self._serial.query(f'MOT:{self._index}:GET?')
+
+        data = response.split(':')
+        enabled = (data[0] == '1')
+        value = int(data[1])
+
+        if not enabled:
+            return MotorPower.COAST
+        return map_to_float(value, -1000, 1000, -1.0, 1.0, precision=3)
+
+    def set_power(self, value: float) -> None:
+        """
+        Set the power of the motor.
+
+        Internally this method maps the power to an integer between
+        -1000 and 1000 so only 3 digits of precision are available.
+
+        :param value: The power of the motor as a float between -1.0 and 1.0
+            or the special values MotorPower.COAST and MotorPower.BRAKE.
+        """
+        if value == MotorPower.COAST:
+            self._serial.write(f'MOT:{self._index}:DISABLE')
+            return
+
+        setpoint = map_to_int(value, -1.0, 1.0, -1000, 1000)
+        self._serial.write(f'MOT:{self._index}:SET:{setpoint}')
+
+    def current(self) -> float:
+        """
+        Read the current draw of the motor.
+
+        :return: The current draw of the motor in amps.
+        """
+        response = self._serial.query(f'MOT:{self._index}:I?')
+        return float(response) / 1000
+
+    def in_fault(self) -> bool:
+        """
+        Check if the motor is in a fault state.
+
+        :return: True if the motor is in a fault state, False otherwise.
+        """
+        response = self._serial.query('*STATUS?')
+        return MotorStatus.from_status_response(response).output_faults[self._index]
+
+    def __repr__(self) -> str:
+        return f"<{self.__class__.__qualname__} index={self._index} {self._serial}>"