From 5a48c91cd3c72cf8dd1278d862aac756f85c58c8 Mon Sep 17 00:00:00 2001 From: Emily Date: Thu, 13 Aug 2026 14:40:32 -0400 Subject: [PATCH 01/17] feat: adding sensor classes for hall effect sensors and brushed motor encoder counter. Updated base sensor class accordingly --- README.md | 12 +- opensourceleg/sensors/base.py | 82 ++++++++++ opensourceleg/sensors/encoderCounter.py | 200 +++++++++++++++++++++++ opensourceleg/sensors/hall.py | 205 ++++++++++++++++++++++++ 4 files changed, 494 insertions(+), 5 deletions(-) create mode 100644 opensourceleg/sensors/encoderCounter.py create mode 100644 opensourceleg/sensors/hall.py diff --git a/README.md b/README.md index 60031703..679cbef6 100644 --- a/README.md +++ b/README.md @@ -53,11 +53,13 @@ This library solves common challenges in developing, testing, and deploying robo The library currently supports the following hardware components: -| Sensors | Unit Tests | Hardware Tests | Benchmarks | Documentation | -| -------------------- | ---------- | -------------- | ---------- | ------------- | -| AS5048B Encoder | ✅ | ✅ | ❌ | ✅ | -| Lord Microstrain IMU | ✅ | ✅ | ❌ | ✅ | -| SRI Loadcell | ✅ | ✅ | ❌ | ✅ | +| Sensors | Unit Tests | Hardware Tests | Benchmarks | Documentation | +| ----------------------- | ----------- | -------------- | ---------- | ------------- | +| AS5048B Encoder | ✅ | ✅ | ❌ | ✅ | +| Lord Microstrain IMU | ✅ | ✅ | ❌ | ✅ | +| SRI Loadcell | ✅ | ✅ | ❌ | ✅ | +| DRV5056 Hall Effect | ❌ | ❌ | ❌ | ❌ | +| LS7366R Encoder Counter | ❌ | ❌ | ❌ | ❌ | | Actuators | Unit Tests | Hardware Tests | Benchmarks | Documentation | | ------------- | ---------- | -------------- | ---------- | ------------- | diff --git a/opensourceleg/sensors/base.py b/opensourceleg/sensors/base.py index 9ceadba1..f1310044 100644 --- a/opensourceleg/sensors/base.py +++ b/opensourceleg/sensors/base.py @@ -214,6 +214,53 @@ def calibrate(self) -> None: pass +class EncoderCounterBase(SensorBase, ABC): + """ + Abstract base class for encoder counter LS7366R. + + Encoder counters interface with incremental encoders. + """ + + # Encoder Counter-specific offline configuration + _OFFLINE_PROPERTIES: ClassVar[list[str]] = [*SensorBase._OFFLINE_PROPERTIES, "position", "velocity"] + _OFFLINE_PROPERTY_DEFAULTS: ClassVar[dict[str, Any]] = { + **SensorBase._OFFLINE_PROPERTY_DEFAULTS, + "position": 0.0, + "velocity": 0.0, + } + + def __init__( + self, + tag: str, + offline: bool = False, + **kwargs: Any, + ) -> None: + """ + Initialize the encoder counter. + """ + super().__init__(tag=tag, offline=offline, **kwargs) # pragma: no cover + + def __repr__(self) -> str: + """ + Return a string representation of the encoder sensor. + + Returns: + str: "EncoderCounterBase" + """ + return "EncoderCounterBase" # pragma: no cover + + @property + @abstractmethod + def count(self) -> float: + """ + Get the current encoder count. + + Returns: + float: The current encoder count. + """ + pass # pragma: no cover + + class EncoderBase(SensorBase, ABC): """ Abstract base class for encoder sensors. @@ -513,5 +560,40 @@ def gyro_z(self) -> float: pass +class HallBase(SensorBase, ABC): + """ + Abstract base class for Hall effect sensors. + + Hall effect sensors measure magnetic fields. + """ + + # hall-specific offline configuration + _OFFLINE_PROPERTIES: ClassVar[list[str]] = [ + *SensorBase._OFFLINE_PROPERTIES, + "field_mT", + ] + _OFFLINE_PROPERTY_DEFAULTS: ClassVar[dict[str, Any]] = { + **SensorBase._OFFLINE_PROPERTY_DEFAULTS, + "field_mT": 0.0, + } + + def __init__(self, tag: str, offline: bool = False, **kwargs: Any) -> None: + """ + Initialize the Hall effect sensor. + """ + super().__init__(tag=tag, offline=offline, **kwargs) # pragma: no cover + + @property + @abstractmethod + def field_mT(self) -> float: + """ + Get the estimated magnetic response + + Returns: + float: Magnetic field in mT. + """ + pass # pragma: no cover + + if __name__ == "__main__": pass diff --git a/opensourceleg/sensors/encoderCounter.py b/opensourceleg/sensors/encoderCounter.py new file mode 100644 index 00000000..e022c7a3 --- /dev/null +++ b/opensourceleg/sensors/encoderCounter.py @@ -0,0 +1,200 @@ +""" +Import LS7366R then create an object by calling enc = LS7366R(csx, clk, byte_mode) +csx is either CE0 or CE1, clk is the speed, byte_mode is the bytemode 1-4 the resolution of your counter. +""" + +from time import sleep +from typing import ClassVar, Final, cast + +import spidev + +from opensourceleg.logging import LOGGER +from opensourceleg.sensors.base import ( + EncoderCounterBase, +) + + +class LS7366R(EncoderCounterBase): + # ------------------------------------------- + # Constants + + # Commands + CLEAR_COUNTER = 0x20 + CLEAR_STATUS = 0x30 + READ_COUNTER = 0x60 + READ_STATUS = 0x70 + WRITE_MODE0 = 0x88 + WRITE_MODE1 = 0x90 + + # Modes + + # May need to be change "QUADRATURE_COUNT_MODE" line depending on the quadrature count mode... look at datasheet. + # These values are in HEX (base 16) whereas the data sheet displays them in binary. + # Datasheet can be found here: https://www.lsicsi.com/pdfs/Data_Sheets/LS7366R.pdf + + # 0x00: non-quadrature count mode. (A = clock, B = direction). + # 0x01: x1 quadrature count mode (one count per quadrature cycle). + # 0x02: x2 quadrature count mode (two counts per quadrature cycle). + # 0x03: x4 quadrature count mode (four counts per quadrature cycle). + + QUADRATURE_COUNT_MODE = 0x03 # originally was 0x00 + + class CounterConfig: + """Counter byte-mode configuration constants for the LS7366R.""" + + FOURBYTE_COUNTER: Final = 0x00 + THREEBYTE_COUNTER: Final = 0x01 + TWOBYTE_COUNTER: Final = 0x02 + ONEBYTE_COUNTER: Final = 0x03 + + MODES: ClassVar[list[int]] = [ONEBYTE_COUNTER, TWOBYTE_COUNTER, THREEBYTE_COUNTER, FOURBYTE_COUNTER] + + # ---------------------------------------------- + # Constructor + + def __init__( + self, + csx: int = 0, + clk: int = 1000000, + byte_mode: int = 4, + max_val: int = 4294967295, # for four byte mode, only correct for four byte mode + spi_bus: int = 0, + offline: bool = False, + tag: str = "EncoderCounter", + ) -> None: + """ + Initialize the LS7366R encoder counter and configure the SPI interface. + + Args: + csx (int): SPI chip select line (CE0 or CE1). Defaults to 0. + clk (int): SPI clock speed in Hz. Defaults to 1000000. + byte_mode (int): Counter resolution in bytes (1 to 4). Defaults to 4. + max_val (int): Maximum counter value for signed conversion. Defaults to 4294967295. + spi_bus (int): SPI bus number. Defaults to 0. + offline (bool): If True, skips SPI initialization. Defaults to False. + tag (str): Human-readable identifier for this encoder instance. Defaults to "EncoderCounter". + """ + + super().__init__(tag=tag, offline=offline) + + self.counter_size = byte_mode # Sets the byte mode that will be used + self.max_val = max_val # Maximum value for the counter, used for signed count conversion + + self.spi = spidev.SpiDev() # Initialize object + self.spi.open(spi_bus, csx) # Which CS line will be used + self.spi.max_speed_hz = clk # Speed of clk (modifies speed transaction) + + # Init the Encoder + LOGGER.info(f"Clearing Encoder CS{csx!s}'s Count...\t") + self.clear_counter() + LOGGER.info(f"Clearing Encoder CS{csx!s}'s Status..\t") + self.clear_status() + + self.spi.xfer2([self.WRITE_MODE0, self.QUADRATURE_COUNT_MODE]) + + sleep(0.1) # Rest + + self.spi.xfer2([self.WRITE_MODE1, self.CounterConfig.MODES[self.counter_size - 1]]) + + def close(self) -> None: + LOGGER.info("Closing Encoder...") + self.clear_counter() + self.clear_status() + self.spi.close() + self.spi = None + + def clear_counter(self) -> str: + """ + Send the clear counter command to the encoder over SPI. + + Returns: + str: "[DONE]" on success. + """ + self.spi.xfer2([self.CLEAR_COUNTER]) + + return "[DONE]" + + def clear_status(self) -> str: + """ + Send the clear status command to the encoder over SPI. + + Returns: + str: "[DONE]" on success. + """ + self.spi.xfer2([self.CLEAR_STATUS]) + + return "[DONE]" + + def read_counter(self) -> int: + """ + Read the current encoder count over SPI. + + Converts the raw multi-byte SPI response into a signed integer based on the configured byte mode. + + Returns: + int: Signed encoder count. + """ + read_transaction = [self.READ_COUNTER] + + read_transaction.extend([0] * self.counter_size) + + data = self.spi.xfer2(read_transaction) + + encoder_count = 0 + for i in range(self.counter_size): + encoder_count = (encoder_count << 8) + data[i + 1] + + if data[1] != 255: + self.encoder_count = encoder_count + else: + self.encoder_count = encoder_count - (self.max_val + 1) + + return self.encoder_count + + def read_status(self) -> int: + """ + Read the status register of the encoder over SPI. + + Returns: + int: 8-bit status register value. + """ + data = self.spi.xfer2([self.READ_STATUS, 0xFF]) + + return cast(int, data[1]) + + def start(self) -> None: + """Start the encoder counter. Not required for this driver.""" + pass + + def stop(self) -> None: + """ + Stop the encoder by closing the SPI connection and clearing registers. + """ + self.close() + LOGGER.info("Motor encoder stopped successfully.") + + def update(self) -> None: + """ + Update the encoder state by reading the latest counter value from SPI. + """ + self.read_counter() + + @property + def count(self) -> int: + """ + Encoder position in counts. + + Returns: + int: Counts reading from the sensor. + """ + return self.read_counter() + + @property + def data(self) -> None: + """Not yet supported by this library.""" + raise NotImplementedError("Data not implemented.") + + @property + def is_streaming(self) -> bool: + """Not yet supported by this library.""" + raise NotImplementedError("Is streaming not implemented.") diff --git a/opensourceleg/sensors/hall.py b/opensourceleg/sensors/hall.py new file mode 100644 index 00000000..c01d639f --- /dev/null +++ b/opensourceleg/sensors/hall.py @@ -0,0 +1,205 @@ +"""Module for using the DRV5056 family of Hall effect sensors.""" + +from typing import ClassVar + +from opensourceleg.logging import LOGGER +from opensourceleg.sensors.base import HallBase + + +class DRV5056(HallBase): + """ + Class for communication with the DRV5056Ax family of Hall effect sensors. + + This class allows reading Hall effect sensors in mT, mV, or V. + """ + + # Class attributes + + # Power supply voltage options + _DRV_VCC_3_3 = 3.3 + _DRV_VCC_5 = 5 + + _QUIESCENT_OFFSET = 0.6 # V + + # Unit conversions + _V_TO_MV = 1000 + + # Helpful dictionaries + _SENSOR_TO_SENS: ClassVar[dict[str, float]] = { + "A1": 200, # mV / mT + "A2": 100, + "A3": 50, + "A4": 25, + "A6": 100, + "A8": 66.6, + "Z1": 200, + "Z2": 100, + "Z3": 50, + "Z4": 25, + } + + _SENS_3_3 = 0.6 + + _SENSOR_TO_RANGE: ClassVar[dict[str, int]] = { + "A1": 20, + "A2": 39, + "A3": 79, + "A4": 158, + "A6": 39, + "A8": 64, + "Z1": 20, + "Z2": 39, + "Z3": 79, + "Z4": 158, + } + + _SENS_3_3_RANGE: ClassVar[dict[str, int]] = { + "A1": 19, + "A2": 39, + "A3": 78, + "A4": 155, + "A6": 39, + "A8": 65, + "Z1": 19, + "Z2": 39, + "Z3": 78, + "Z4": 155, + } + + _SENSOR_TO_COMP: ClassVar[dict[str, float]] = { + "A1": 0.0012, + "A2": 0.0012, + "A3": 0.0012, + "A4": 0.0012, + "A6": 0.0012, + "A8": 0.0012, + "Z1": 0.0, + "Z2": 0.0, + "Z3": 0.0, + "Z4": 0.0, + } + + def __init__( + self, + tag: str = "DRV5056A1", + offline: bool = False, + sensor_num: str = "A1", + t_a: int = 23, + supply_voltage: float = 5, + ): + """ + Initialize the DRV5056 instance. + + Args: + tag (str): Identifier for the Hall effect instance. Default is "DRV5056A1". + offline (bool): If True, the ADC operates in offline mode. Default is False. + sensor_num (str): Hall effect sensor part number. Default is A1. + t_a (int): Ambient temperature. Default is 23 degrees Celsius. + supply_voltage (float): Power supply voltage. Default is 5 V. + + """ + if offline: + exit(1) + + super().__init__(tag=tag, offline=offline) + + self._sensor_num = sensor_num + self._t_a = t_a + self._supply_voltage = supply_voltage + + def __repr__(self) -> str: + return "DRV5056" + + def configure( + self, + ) -> None: + """ + Configure Hall effect settings based on part number and supply voltage. + + Raises: + ValueError: If sensor_num is not a supported part number. + ValueError: If supply voltage is outside the supported range. + """ + # --- SENSITIVITY --- + if self._sensor_num not in self._SENSOR_TO_SENS: + raise ValueError( + f"Unsupported sensor={self._sensor_num}. Choose from {sorted(self._SENSOR_TO_SENS.keys())}" + ) + self.base_sensitivity = self._SENSOR_TO_SENS[self._sensor_num] + self.lower_volt_sensitivity = self._SENS_3_3 * self.base_sensitivity + + # --- RANGE --- + self.base_range = self._SENSOR_TO_RANGE[self._sensor_num] + self.lower_volt_range = self._SENS_3_3_RANGE[self._sensor_num] + + # --- TEMPERATURE COMPENSATION --- + self._s_tc = self._SENSOR_TO_COMP[self._sensor_num] + + # --- VCC --- + if self._supply_voltage != self._DRV_VCC_3_3 and self._supply_voltage != self._DRV_VCC_5: + if 4.5 <= self._supply_voltage <= 5.5: + self.range = self.base_range + self._sensitivity = self.base_sensitivity * self._supply_voltage / self._DRV_VCC_5 + elif 3 <= self._supply_voltage <= 3.6: + self.range = self.lower_volt_range + self._sensitivity = self.lower_volt_sensitivity * self._supply_voltage / self._DRV_VCC_3_3 + else: + raise ValueError("Supply voltage out of range.") + elif self._supply_voltage == self._DRV_VCC_3_3: + self.range = self.lower_volt_range + self._sensitivity = self.lower_volt_sensitivity + elif self._supply_voltage == self._DRV_VCC_5: + self.range = self.base_range + self._sensitivity = self.base_sensitivity + + def start(self) -> None: + """Start the Hall effect sensor by enabling the data stream.""" + self._streaming = True + + def stop(self) -> None: + """Stop the Hall effect sensor by disabling the data stream.""" + self._streaming = False + + def update(self) -> None: + """Calculate the estimated magnetic response.""" + self.field_strength = (self.voltage * self._V_TO_MV - self._QUIESCENT_OFFSET) / ( + self._sensitivity * (1 + (self._s_tc * (self._t_a - 25))) + ) + if self.range == self.field_strength: + LOGGER.error("Careful. The sensor may be out of range and your magnetic field may be higher.") + + @property + def is_streaming(self) -> bool: + """ + Check if the Hall is currently streaming data. + + Returns: + bool: True if streaming, False otherwise. + """ + return self._streaming + + @property + def voltage(self) -> float: + """ + Get the latest Hall effect data. + + Returns: + float: Voltage reading from the sensor. + """ + return 0.0 + + @property + def field_mT(self) -> float: + """ + Get the latest Hall effect data in millitesla. + + Returns: + float: Magnetic field strength in millitesla (mT). + """ + self.update() + return self.field_strength + + @property + def data(self) -> float: + """Not yet supported by this library.""" + raise NotImplementedError("Data not implemented.") From 7f0090787e762f3304ac8a69f15977ea21510d12 Mon Sep 17 00:00:00 2001 From: Emily Date: Thu, 13 Aug 2026 14:42:34 -0400 Subject: [PATCH 02/17] feat: add adc class ADS114S0x --- opensourceleg/sensors/adc.py | 1122 +++++++++++++++++++++++++++++++++- pyproject.toml | 1 + uv.lock | 36 +- 3 files changed, 1140 insertions(+), 19 deletions(-) diff --git a/opensourceleg/sensors/adc.py b/opensourceleg/sensors/adc.py index 724bf67e..80cde7bb 100644 --- a/opensourceleg/sensors/adc.py +++ b/opensourceleg/sensors/adc.py @@ -1,17 +1,1113 @@ """ -Module for communicating with the ADS131M0x family of ADC chips. +Module for communicating with the ADS131M0x and ADS114S0x family of ADC chips. """ import math +from dataclasses import dataclass +from enum import Enum from time import sleep -from typing import Any, ClassVar, Optional +from typing import Any, Callable, ClassVar, Optional, cast import numpy as np +from gpiozero import DigitalInputDevice from opensourceleg.logging import LOGGER from opensourceleg.sensors.base import ADCBase +class ADS114S0x(ADCBase): + """ + Class for communication with the ADS114S0x family of ADC chips. + + This class allows configuration of the ADS114S0x chips and reading ADC values in millivolts. + """ + + # Class attributes + + # Constants + _NUM_REGISTERS = 18 + _ADS124S08_FCLK = 4096000 # Standard internal clock frequency + _ADS114S08_BITRES = 16 # ADC resolution + + # Data lengths + _DATA_LENGTH = 3 # Conversion data total bytes + _COMMAND_LENGTH = 2 # Register read/write command length + _STATUS_LENGTH = 1 # Status length in bytes + _CRC_LENGTH = 1 # CRC length in bytes + _RDATA_COMMAND_LENGTH = 1 # RDATA command length + + # Internal reference voltage + _INT_VREF = 2.5 + + # Timing delays + _DELAY_4TCLK = 1 # microseconds + _DELAY_4096TCLK = int(4096.0 * 1000000 / _ADS124S08_FCLK) + _DELAY_2p2MS = int(0.0022 * 1000000) + + # SPI Commands + _OPCODE_NOP = 0x00 + _OPCODE_WAKEUP = 0x02 + _OPCODE_POWERDOWN = 0x04 + _OPCODE_RESET = 0x06 + _OPCODE_START = 0x08 + _OPCODE_STOP = 0x0A + _OPCODE_SYOCAL = 0x16 + _OPCODE_SYGCAL = 0x17 + _OPCODE_SFOCAL = 0x19 + _OPCODE_RDATA = 0x12 + _OPCODE_RREG = 0x20 + _OPCODE_WREG = 0x40 + _OPCODE_RWREG_MASK = 0x1F + + # Read mode enum + class ReadMode(Enum): + DIRECT = 0 + COMMAND = 1 + + # Register addresses + _REG_ADDR_ID = 0x00 + _REG_ADDR_STATUS = 0x01 + _REG_ADDR_INPMUX = 0x02 + _REG_ADDR_PGA = 0x03 + _REG_ADDR_DATARATE = 0x04 + _REG_ADDR_REF = 0x05 + _REG_ADDR_IDACMAG = 0x06 + _REG_ADDR_IDACMUX = 0x07 + _REG_ADDR_VBIAS = 0x08 + _REG_ADDR_SYS = 0x09 + # ADS114S08 calibration regs (16-bit each) + _REG_ADDR_RESERVED0A = 0x0A + _REG_ADDR_OFCAL0 = 0x0B + _REG_ADDR_OFCAL1 = 0x0C + _REG_ADDR_RESERVED0D = 0x0D + _REG_ADDR_FSCAL0 = 0x0E + _REG_ADDR_FSCAL1 = 0x0F + _REG_ADDR_GPIODAT = 0x10 + _REG_ADDR_GPIOCON = 0x11 + + # Register default values + _ID_DEFAULT = 0x00 + _STATUS_DEFAULT = 0x80 + _INPMUX_DEFAULT = 0x01 + _PGA_DEFAULT = 0x00 + _DATARATE_DEFAULT = 0x14 + _REF_DEFAULT = 0x10 + _IDACMAG_DEFAULT = 0x00 + _IDACMUX_DEFAULT = 0xFF + _VBIAS_DEFAULT = 0x00 + _SYS_DEFAULT = 0x10 + _OFCAL0_DEFAULT = 0x00 + _OFCAL1_DEFAULT = 0x00 + _FSCAL0_DEFAULT = 0x00 + _FSCAL1_DEFAULT = 0x40 + _RESERVED0A_DEFAULT = 0x00 + _RESERVED0D_DEFAULT = 0x00 + _GPIODAT_DEFAULT = 0x00 + _GPIOCON_DEFAULT = 0x00 + + # Status register masks + _ADS_nRDY_MASK = 0x40 + _ADS_FL_POR_MASK = 0x80 + + # SYS register masks + _ADS_SENDSTATUS_MASK = 0x01 + _ADS_CRC_MASK = 0x02 + + # Input multiplexer settings + _ADS_P_AIN0 = 0x00 + _ADS_P_AIN1 = 0x10 + _ADS_P_AIN2 = 0x20 + _ADS_P_AIN3 = 0x30 + _ADS_P_AIN4 = 0x40 + _ADS_P_AIN5 = 0x50 + _ADS_P_AIN6 = 0x60 + _ADS_P_AIN7 = 0x70 + _ADS_P_AIN8 = 0x80 + _ADS_P_AIN9 = 0x90 + _ADS_P_AIN10 = 0xA0 + _ADS_P_AIN11 = 0xB0 + _ADS_P_AINCOM = 0xC0 + + _ADS_N_AIN0 = 0x00 + _ADS_N_AIN1 = 0x01 + _ADS_N_AIN2 = 0x02 + _ADS_N_AIN3 = 0x03 + _ADS_N_AIN4 = 0x04 + _ADS_N_AIN5 = 0x05 + _ADS_N_AIN6 = 0x06 + _ADS_N_AIN7 = 0x07 + _ADS_N_AIN8 = 0x08 + _ADS_N_AIN9 = 0x09 + _ADS_N_AIN10 = 0x0A + _ADS_N_AIN11 = 0x0B + _ADS_N_AINCOM = 0x0C + + # PGA settings + _ADS_DELAY_14 = 0x00 + _ADS_DELAY_25 = 0x20 + _ADS_DELAY_64 = 0x40 + _ADS_DELAY_256 = 0x60 + _ADS_DELAY_1024 = 0x80 + _ADS_DELAY_2048 = 0xA0 + _ADS_DELAY_4096 = 0xC0 + _ADS_DELAY_1 = 0xE0 + + _ADS_PGA_BYPASS = 0x00 + _ADS_PGA_ENABLED = 0x08 + + _ADS_GAIN_1 = 0x00 + _ADS_GAIN_2 = 0x01 + _ADS_GAIN_4 = 0x02 + _ADS_GAIN_8 = 0x03 + _ADS_GAIN_16 = 0x04 + _ADS_GAIN_32 = 0x05 + _ADS_GAIN_64 = 0x06 + _ADS_GAIN_128 = 0x07 + _ADS_GAIN_MASK = 0x07 + + # Data rate settings + _ADS_GLOBALCHOP = 0x80 + _ADS_CLKSEL_EXT = 0x40 + _ADS_CONVMODE_SS = 0x20 + _ADS_CONVMODE_CONT = 0x00 + _ADS_FILTERTYPE_LL = 0x10 + + _ADS_DR_2_5 = 0x00 + _ADS_DR_5 = 0x01 + _ADS_DR_10 = 0x02 + _ADS_DR_16 = 0x03 + _ADS_DR_20 = 0x04 + _ADS_DR_50 = 0x05 + _ADS_DR_60 = 0x06 + _ADS_DR_100 = 0x07 + _ADS_DR_200 = 0x08 + _ADS_DR_400 = 0x09 + _ADS_DR_800 = 0x0A + _ADS_DR_1000 = 0x0B + _ADS_DR_2000 = 0x0C + _ADS_DR_4000 = 0x0D + + # Reference settings + _ADS_FLAG_REF_DISABLE = 0x00 + _ADS_FLAG_REF_EN_L0 = 0x40 + _ADS_FLAG_REF_EN_BOTH = 0x80 + _ADS_FLAG_REF_EN_10M = 0xC0 + _ADS_REFP_BYP_DISABLE = 0x20 + _ADS_REFP_BYP_ENABLE = 0x00 + _ADS_REFN_BYP_DISABLE = 0x10 + _ADS_REFN_BYP_ENABLE = 0x00 + _ADS_REFSEL_P0 = 0x00 + _ADS_REFSEL_P1 = 0x04 + _ADS_REFSEL_INT = 0x08 + _ADS_REFINT_OFF = 0x00 + _ADS_REFINT_ON_PDWN = 0x01 + _ADS_REFINT_ON_ALWAYS = 0x02 + + # IDAC settings + _ADS_FLAG_RAIL_ENABLE = 0x80 + _ADS_FLAG_RAIL_DISABLE = 0x00 + _ADS_PSW_OPEN = 0x00 + _ADS_PSW_CLOSED = 0x40 + _ADS_IDACMAG_OFF = 0x00 + _ADS_IDACMAG_10 = 0x01 + _ADS_IDACMAG_50 = 0x02 + _ADS_IDACMAG_100 = 0x03 + _ADS_IDACMAG_250 = 0x04 + _ADS_IDACMAG_500 = 0x05 + _ADS_IDACMAG_750 = 0x06 + _ADS_IDACMAG_1000 = 0x07 + _ADS_IDACMAG_1500 = 0x08 + _ADS_IDACMAG_2000 = 0x09 + + # IDAC multiplexer settings + _ADS_IDAC2_A0 = 0x00 + _ADS_IDAC2_A1 = 0x10 + _ADS_IDAC2_A2 = 0x20 + _ADS_IDAC2_A3 = 0x30 + _ADS_IDAC2_A4 = 0x40 + _ADS_IDAC2_A5 = 0x50 + _ADS_IDAC2_A6 = 0x60 + _ADS_IDAC2_A7 = 0x70 + _ADS_IDAC2_A8 = 0x80 + _ADS_IDAC2_A9 = 0x90 + _ADS_IDAC2_A10 = 0xA0 + _ADS_IDAC2_A11 = 0xB0 + _ADS_IDAC2_AINCOM = 0xC0 + _ADS_IDAC2_OFF = 0xF0 + + _ADS_IDAC1_A0 = 0x00 + _ADS_IDAC1_A1 = 0x01 + _ADS_IDAC1_A2 = 0x02 + _ADS_IDAC1_A3 = 0x03 + _ADS_IDAC1_A4 = 0x04 + _ADS_IDAC1_A5 = 0x05 + _ADS_IDAC1_A6 = 0x06 + _ADS_IDAC1_A7 = 0x07 + _ADS_IDAC1_A8 = 0x08 + _ADS_IDAC1_A9 = 0x09 + _ADS_IDAC1_A10 = 0x0A + _ADS_IDAC1_A11 = 0x0B + _ADS_IDAC1_AINCOM = 0x0C + _ADS_IDAC1_OFF = 0x0F + + # VBIAS settings + _ADS_VBIAS_LVL_DIV2 = 0x00 + _ADS_VBIAS_LVL_DIV12 = 0x80 + _ADS_VB_AINC = 0x40 + _ADS_VB_AIN5 = 0x20 + _ADS_VB_AIN4 = 0x10 + _ADS_VB_AIN3 = 0x08 + _ADS_VB_AIN2 = 0x04 + _ADS_VB_AIN1 = 0x02 + _ADS_VB_AIN0 = 0x01 + + # System monitor settings + _ADS_SYS_MON_OFF = 0x00 + _ADS_SYS_MON_SHORT = 0x20 + _ADS_SYS_MON_TEMP = 0x40 + _ADS_SYS_MON_ADIV4 = 0x60 + _ADS_SYS_MON_DDIV4 = 0x80 + _ADS_SYS_MON_BCS_2 = 0xA0 + _ADS_SYS_MON_BCS_1 = 0xC0 + _ADS_SYS_MON_BCS_10 = 0xE0 + _ADS_CALSAMPLE_1 = 0x00 + _ADS_CALSAMPLE_4 = 0x08 + _ADS_CALSAMPLE_8 = 0x10 + _ADS_CALSAMPLE_16 = 0x18 + _ADS_TIMEOUT_DISABLE = 0x00 + _ADS_TIMEOUT_ENABLE = 0x04 + _ADS_CRC_DISABLE = 0x00 + _ADS_CRC_ENABLE = 0x02 + _ADS_SENDSTATUS_DISABLE = 0x00 + _ADS_SENDSTATUS_ENABLE = 0x01 + + # SPI Configuration + _SPI_SPEED = 2000000 # 2 MHz + _SPI_BUS = 1 + _SPI_DEVICE = 0 + + # Constants + _HIGH = True + _LOW = False + + # Internal variables + _spi = None + + # CRC Configuration + _CRC_LOOKUP = True # Use lookup table method + _CRC_INITIAL_SEED = 0x00 + _CRC_POLYNOMIAL = 0x07 # CRC-8-ATM (HEC) polynomial: X^8 + X^2 + X + 1 + + # Internal variables + _initialized = False + _crc_lookup_table = [0] * 256 + + _MAX_CHANNELS = 12 + + def __init__( + self, + tag: str = "ADS114S08", + spi_bus: int = 0, + spi_cs: int = 0, + data_rate: int = 400, + pga_gain: int = 1, + voltage_reference: float = _INT_VREF, + drdy: int = 16, + offline: bool = False, + ) -> None: + """ + Initialize the ADS114S0x instance. + + Args: + tag (str): Identifier for the ADC instance. Default is "ADS114S0x". + spi_bus (int): SPI bus number. Default is 0. + spi_cs (int): SPI chip select line. Default is 0. + data_rate (int): Sampling rate in Hz. Default is 500 Hz. + pga_gain (int): Default is 1. + voltage_reference (float): Reference voltage in volts. Default is 2.5 V. + drdy (int): GPIO pin number for the data-ready signal. Defaults to 16. + offline (bool): If True, the ADC operates in offline mode. Default is False. + + Raises: + ImportError: If spidev is not installed and offline is False. + """ + + try: + import spidev + + self._spi = spidev.SpiDev() + except ImportError: + LOGGER.error("spidev is not installed. Please install it to use this module.") + + if not offline: + exit(1) + + super().__init__(tag=tag, offline=offline) + + self._spi_bus = spi_bus + self._spi_cs = spi_cs + self._pga_gain = pga_gain + self._voltage_reference = voltage_reference + self._streaming = False + self._data_rate = data_rate + self._drdy = DigitalInputDevice(drdy, pull_up=False) + self._channels: dict[str, ChannelConfig] = {} + self._data: Optional[list[float]] = None + self._register_map = [0] * self._NUM_REGISTERS + LOGGER.info(f"ADC initialized with tag: {self._tag}") + + def __repr__(self) -> str: + return "ADS114S0x" + + # Functions Required by SensorBase and dependencies + def start(self) -> None: + """ + Start the ADC by opening the SPI port, resetting the device, and confirming reading and writing + """ + LOGGER.info("Starting ADC...") + self.init_spi() + + self.delay_us(self._DELAY_2p2MS) + self.reset() + self.restore_register_defaults() + + # Configure initial device register settings + self.write_single_register(self._REG_ADDR_STATUS, 0x00) # Reset POR event + self._set_device_state(1) + LOGGER.info("ADC started successfully.") + + def _set_device_state(self, state: int) -> None: + """ + Set the internal state of the ADC device. + + Args: + state (int): The desired state: + 0 -- Standby mode. + 1 -- Continuous Conversion Mode. + """ + if state == 0: + self._streaming = False + elif state == 1: + self._streaming = True + + def stop(self) -> None: + """ + Stop the ADC by transitioning to standby mode and closing the SPI port. + """ + LOGGER.info("Stopping ADC...") + self.send_stop() + self.cleanup() + LOGGER.info("ADC stopped successfully.") + + def update(self) -> None: + """ + Update the ADC data by reading the latest voltage values in millivolts. + Attempts to read a maximum of 1000 times before throwing an error. + + Raises: + RuntimeError: If the ADC does not become ready within 1000 attempts. + """ + + max_attempts = 1000 + attempts = 0 + + while self._ready_to_read() is False: + sleep(0.001) + attempts += 1 + if attempts > max_attempts: + raise RuntimeError( + "Couldn't connect to the ADC, please ensure that the device is connected and powered on." + ) + + self._data = self._read_data_millivolts() + + def _ready_to_read(self) -> bool: + """ + Check if all ADC channels are ready for a new data read. + + Returns: + bool: True if the status register indicates readiness; otherwise, False. + """ + + reply = self.read_single_register(address=self._REG_ADDR_STATUS) + + return not reply & self._ADS_nRDY_MASK + + def _read_data_millivolts(self) -> list[float] | None: + """ + Read all configured channels and return their values in millivolts. + + Returns: + list[float] | None: Millivolt readings for each configured channel, + or None if no channels have been configured. + """ + if not self._channels: + LOGGER.info("No channels have been configured for reading. Use ChannelConfig.") + return None + + row: list[float] = [] + for ch in self._channels.values(): + self.set_mux_single_ended(ch.ain_pos_code) + self.discard_settling_reads(timeout_ms=1000) + self.start_conversions() + code16, _ = self.wait_and_read_code16() + volts = self.code16_to_volts(code16) + millivolts = volts * 1000 + + if ch.postprocess is not None: + millivolts = ch.postprocess(millivolts) + + row += [millivolts] + + return row + + # Properties required by SensorBase + @property + def is_streaming(self) -> bool: + """ + Check if the ADC is currently streaming data. + + Returns: + bool: True if streaming, False otherwise. + """ + return self._streaming + + @property + def data(self) -> list[float] | None: + """ + Get the latest ADC data in millivolts. + + Returns: + np.ndarray: Array of voltage readings for each channel. + """ + return self._data + + # Functions replacing ADCBase functions + def reset(self) -> None: + """ + Sends RESET command through SPI + """ + self.send_command(self._OPCODE_RESET) + + # Functions transferred from VSO-CODEBASE-DEV repo: ads114s08.py + def get_register_value(self, address: int) -> int: + """ + Getter function to access the register map array. + + Args: + address: The 8-bit register address + + Returns: + The 8-bit register value + """ + if address >= self._NUM_REGISTERS: + raise ValueError("Register address out of range") + return self._register_map[address] + + def is_sendstat_set(self) -> bool: + """Check if SENDSTAT bit is set in SYS register.""" + return bool(self.get_register_value(self._REG_ADDR_SYS) & self._ADS_SENDSTATUS_MASK) + + def is_crc_set(self) -> bool: + """Check if CRC bit is set in SYS register.""" + return bool(self.get_register_value(self._REG_ADDR_SYS) & self._ADS_CRC_MASK) + + def read_single_register(self, address: int) -> int: + """ + Reads contents of a single register at the specified address + + Args: + address: Address of the register to be read + + Returns: + 8-bit register contents + """ + if address >= self._NUM_REGISTERS: + raise ValueError("Register address out of range") + + # Build TX array + data_tx = [self._OPCODE_RREG | (address & self._OPCODE_RWREG_MASK), 0, 0] + + data_rx = self.spi_send_receive_arrays(data_tx, self._COMMAND_LENGTH + 1) + + # Update register array and return result + self._register_map[address] = data_rx[self._COMMAND_LENGTH] + return data_rx[self._COMMAND_LENGTH] + + def read_multiple_registers(self, start_address: int = 0x00, count: int = 17) -> None: + """ + Reads a group of registers starting at the specified address + Use get_register_value() to retrieve the read values + + Args: + start_address: Register address to start reading (HEX) + count: Number of registers to read + """ + if start_address + count > self._NUM_REGISTERS: + raise ValueError("Register address(es) out of range") + + # Build TX array + data_tx = [self._OPCODE_RREG | (start_address & self._OPCODE_RWREG_MASK), count - 1] + data_tx.extend([0] * count) + + data_rx = self.spi_send_receive_arrays(data_tx, self._COMMAND_LENGTH + count) + + # Store received register data + for i in range(count): + self._register_map[i + start_address] = data_rx[self._COMMAND_LENGTH + i] + + def write_single_register(self, address: int, data: int) -> None: + """ + Write data to a single register at the specified address + + Args: + address: Register address to write + data: 8-bit data to write + """ + if address >= self._NUM_REGISTERS: + raise ValueError("Register address out of range") + + # Build TX array + data_tx = [self._OPCODE_WREG | (address & self._OPCODE_RWREG_MASK), 0, data & 0xFF] + + self.spi_send_receive_arrays(data_tx, self._COMMAND_LENGTH + 1) + + # Update register array + self._register_map[address] = data & 0xFF + + def write_multiple_registers(self, start_address: int, count: int, reg_data: list[int]) -> None: + """ + Write data to a group of registers + + Args: + start_address: Register address to start writing + count: Number of registers to write + reg_data: List of data to write (element zero is data for starting address) + """ + if start_address + count > self._NUM_REGISTERS: + raise ValueError("Register address(es) out of range") + if reg_data is None: + raise ValueError("reg_data cannot be None") + + # Build TX array + data_tx = [self._OPCODE_WREG | (start_address & self._OPCODE_RWREG_MASK), count - 1] + + for i in range(start_address, start_address + count): + data_tx.append(reg_data[i - start_address] & 0xFF) + self._register_map[i] = reg_data[i - start_address] & 0xFF + + self.spi_send_receive_arrays(data_tx, self._COMMAND_LENGTH + count) + + def send_command(self, op_code: int) -> None: + """ + Sends the specified SPI command to the ADC + + Args: + op_code: SPI command byte + """ + if op_code == self._OPCODE_RREG: + raise ValueError("Use read_single_register() or read_multiple_registers()") + if op_code == self._OPCODE_WREG: + raise ValueError("Use write_single_register() or write_multiple_registers()") + + self.spi_send_receive_byte(op_code) + + # Check for RESET command + if op_code == self._OPCODE_RESET: + self.delay_us(self._DELAY_4096TCLK) # Must wait 4096 tCLK after reset + self.restore_register_defaults() + + def start_conversions(self) -> None: + """ + Wakes the device from power-down and starts continuous conversions + """ + # Wakeup device if in POWERDOWN + self.send_wakeup() + + # Begin continuous conversions + # If using START pin control, uncomment this: + # hal.set_start(hal.HIGH) + # Otherwise use SPI command: + self.send_start() + + def read_converted_data(self, mode: ReadMode = ReadMode.DIRECT) -> tuple[int, Optional[int]]: + """ + Sends the read command and retrieves STATUS (if enabled) and data + Call this function after /DRDY goes low + + Args: + mode: Direct or Command read mode + + Returns: + Tuple of (32-bit sign-extended conversion result, status byte or None) + """ + # Determine byte length and data position + status_byte_enabled = self.is_sendstat_set() + crc_enabled = self.is_crc_set() + + byte_options = (status_byte_enabled << 1) | crc_enabled + + if byte_options == 0: # No STATUS and no CRC + byte_length = self._DATA_LENGTH + data_position = 0 + elif byte_options == 1: # No STATUS and CRC + byte_length = self._DATA_LENGTH + self._CRC_LENGTH + data_position = 0 + elif byte_options == 2: # STATUS and no CRC + byte_length = self._STATUS_LENGTH + self._DATA_LENGTH + data_position = 1 + else: # STATUS and CRC + byte_length = self._STATUS_LENGTH + self._DATA_LENGTH + self._CRC_LENGTH + data_position = 1 + + # Build TX array + data_tx = [0] * (self._RDATA_COMMAND_LENGTH + byte_length) + + if mode == self.ReadMode.COMMAND: + data_tx[0] = self._OPCODE_RDATA + byte_length += 1 + data_position += 1 + + data_rx = self.spi_send_receive_arrays(data_tx, byte_length) + + # Parse status byte if enabled + status = None + if status_byte_enabled: + status = data_rx[data_position - 1] + + # Verify CRC if enabled + if crc_enabled: + if status_byte_enabled: + data = [ + data_rx[data_position - 1], # status + data_rx[data_position], # msb + data_rx[data_position + 1], # mid + data_rx[data_position + 2], # lsb + data_rx[data_position + 3], # crc + ] + error = bool(self.get_crc(data, 5)) + else: + data = [ + data_rx[data_position], # msb + data_rx[data_position + 1], # mid + data_rx[data_position + 2], # lsb + data_rx[data_position + 3], # crc + ] + error = bool(self.get_crc(data, 4)) + + if error: + raise ValueError("CRC error in converted data") + + # --- ADS114S08: 3 data bytes are returned, but the ADC result is 16-bit. + # Treat the 3 bytes as a signed 24-bit container, then shift down to 16-bit. + msb = data_rx[data_position] + mid = data_rx[data_position + 1] + lsb = data_rx[data_position + 2] # typically padding / low byte in this framing + + raw24 = (msb << 16) | (mid << 8) | lsb + if msb & 0x80: # sign bit of the 24-bit container + raw24 -= 1 << 24 + + code16 = raw24 >> 8 # signed 16-bit conversion code (-32768..32767) + + return (code16, status) + + def restore_register_defaults(self) -> None: + """ + Updates the register_map array to its default values + Should be called after powering up or resetting the device + """ + self._register_map[self._REG_ADDR_ID] = self._ID_DEFAULT + self._register_map[self._REG_ADDR_STATUS] = self._STATUS_DEFAULT + self._register_map[self._REG_ADDR_INPMUX] = self._INPMUX_DEFAULT + self._register_map[self._REG_ADDR_PGA] = self._PGA_DEFAULT + self._register_map[self._REG_ADDR_DATARATE] = self._DATARATE_DEFAULT + self._register_map[self._REG_ADDR_REF] = self._REF_DEFAULT + self._register_map[self._REG_ADDR_IDACMAG] = self._IDACMAG_DEFAULT + self._register_map[self._REG_ADDR_IDACMUX] = self._IDACMUX_DEFAULT + self._register_map[self._REG_ADDR_VBIAS] = self._VBIAS_DEFAULT + self._register_map[self._REG_ADDR_SYS] = self._SYS_DEFAULT + self._register_map[self._REG_ADDR_RESERVED0A] = self._RESERVED0A_DEFAULT + self._register_map[self._REG_ADDR_OFCAL0] = self._OFCAL0_DEFAULT + self._register_map[self._REG_ADDR_OFCAL1] = self._OFCAL1_DEFAULT + self._register_map[self._REG_ADDR_RESERVED0D] = self._RESERVED0D_DEFAULT + self._register_map[self._REG_ADDR_FSCAL0] = self._FSCAL0_DEFAULT + self._register_map[self._REG_ADDR_FSCAL1] = self._FSCAL1_DEFAULT + self._register_map[self._REG_ADDR_GPIODAT] = self._GPIODAT_DEFAULT + self._register_map[self._REG_ADDR_GPIOCON] = self._GPIOCON_DEFAULT + + # Functions transferred from VSO-CODEBASE-DEV repo: hal.py + def init_spi(self) -> None: + """ + Configures the Raspberry Pi's SPI peripheral for interfacing with the ADC + + Raises: + RuntimeError: If the SPI device is not initialized. + + Note: ADS124S08 operates in SPI mode 1 (CPOL = 0, CPHA = 1) + """ + if self._spi is None: + raise RuntimeError("SPI device is not initialized. Ensure spidev is installed.") + self._spi.open(self._spi_bus, self._spi_cs) + self._spi.max_speed_hz = self._SPI_SPEED + self._spi.mode = 0b01 # SPI Mode 1 (CPOL=0, CPHA=1) + self._spi.bits_per_word = 8 + + def cleanup(self) -> None: + """ + Cleanup SPI resources + """ + if self._spi is not None: + self._spi.close() + self._spi = None + + def delay_us(self, delay_time_us: int) -> None: + """ + Provides a timing delay with microsecond resolution + + Args: + delay_time_us: Number of microseconds to delay + """ + sleep(delay_time_us / 1000000.0) + + def wait_for_drdy_htol(self, timeout_ms: int) -> bool: + """ + Waits for conversion to complete by sleeping one conversion period. + + In single-shot mode with the low-latency filter, each conversion takes 1/data_rate seconds. + A 1.5x multiplier provides a safe margin. + + Args: + timeout_ms: Unused; kept for API compatibility. + + Returns: + Always True. + """ + sleep(1.5 / self._data_rate) + return True + + def send_start(self) -> None: + """ + Sends START command through SPI + """ + self.send_command(self._OPCODE_START) + + def send_stop(self) -> None: + """ + Sends STOP command through SPI + """ + self.send_command(self._OPCODE_STOP) + + def send_wakeup(self) -> None: + """ + Sends WAKEUP command through SPI + """ + self.send_command(self._OPCODE_WAKEUP) + + def send_powerdown(self) -> None: + """ + Sends POWERDOWN command through SPI + """ + self.send_command(self._OPCODE_POWERDOWN) + + def spi_send_receive_arrays(self, data_tx: list[int], byte_length: int) -> list[int]: + """ + Sends SPI commands to ADC and returns response + + Args: + data_tx: List of SPI data to send on MOSI + byte_length: Number of bytes to send/receive on SPI + + Returns: + List of received bytes from MISO + + Raises: + RuntimeError: If the SPI device is not initialized. + """ + if self._spi is None: + raise RuntimeError("SPI device is not initialized.") + + # Ensure data_tx has the correct length + tx_data = data_tx[:byte_length] + + rx_data = self._spi.xfer2(tx_data) + + return cast(list[int], rx_data) + + def spi_send_receive_byte(self, data_tx: int) -> int: + """ + Sends a single byte to ADC and returns response + + Args: + data_tx: Byte to send on MOSI + + Returns: + Received byte from MISO + + Raises: + RuntimeError: If the SPI device is not initialized. + """ + if self._spi is None: + raise RuntimeError("SPI device is not initialized.") + + rx_data = self._spi.xfer2([data_tx & 0xFF]) + + return cast(int, rx_data[0]) + + # Functions transferred from VSO-CODEBASE-DEV repo crc.py + def init_crc(self) -> None: + """ + Initializes CRC module and creates lookup table (if using lookup method) + """ + if self._CRC_LOOKUP: + self._init_table() + self._initialized = True + + def get_crc(self, data_bytes: list[int], number_bytes: int) -> int: + """ + Performs CRC lookup or calculation + + Args: + data_bytes: List of data bytes to process + number_bytes: Number of bytes in array to process + + Returns: + CRC value of the calculation + + Example: + # To calculate the CRC of a 3-byte message: + crc = get_crc(data, 3) + + # To test a 4-byte message with a CRC byte: + error = bool(get_crc(data, 4)) + """ + if self._CRC_LOOKUP: + if not self._initialized: + self._init_table() + return self._lookup_crc(data_bytes, number_bytes) + else: + return self._calculate_crc(data_bytes, number_bytes) + + def _init_table(self) -> None: + """ + Creates lookup table in memory using byte wide computation in a 256 element array + """ + for i in range(256): + value = i & 0xFF + self._crc_lookup_table[i] = self._calculate_crc([value], 1) + + def _lookup_crc(self, data_bytes: list[int], number_bytes: int) -> int: + """ + Performs CRC lookup operation in byte increments + + Args: + data_bytes: List of data bytes (little endian) + number_bytes: Number of bytes in array to process + + Returns: + CRC value of the calculation + """ + crc = self._CRC_INITIAL_SEED & 0xFF + + for i in range(number_bytes): + crc = self._crc_lookup_table[crc ^ (data_bytes[i] & 0xFF)] + + return crc & 0xFF + + def _calculate_crc(self, data_bytes: list[int], number_bytes: int) -> int: + """ + Calculates the CRC for the selected CRC polynomial + + Args: + data_bytes: List of data bytes + number_bytes: Number of bytes to be used in CRC calculation + + Returns: + CRC value of the calculation + """ + crc = self._CRC_INITIAL_SEED & 0xFF + + # Loop through all bytes in the data_bytes array + for byte_index in range(number_bytes): + # Point to most significant bit + bit_index = 0x80 + + # Loop through all bits in the current byte + while bit_index > 0: + # Check MSB's of data and crc + data_msb = bool(data_bytes[byte_index] & bit_index) + crc_msb = bool(crc & 0x80) + + # Update crc register + crc = (crc << 1) & 0xFF + if data_msb ^ crc_msb: + crc ^= self._CRC_POLYNOMIAL + + # Shift MSb pointer to the next data bit + bit_index >>= 1 + + return crc & 0xFF + + # Functions transferred from VSO_CODEBASE_DEV repo adc_common.py + def set_mux_single_ended(self, pos_code: int, neg_code: Optional[int] = None) -> None: + """ + Configure the input multiplexer for a single-ended measurement. + + INPMUX: upper nibble = positive input, lower nibble = negative input. + For example, to read AIN3 against AINCOM, pass _ADS_P_AIN3 as pos_code. + + Args: + pos_code (int): Positive input channel code (e.g. _ADS_P_AIN3). + neg_code (int, optional): Negative input channel code. Defaults to _ADS_N_AINCOM. + """ + if neg_code is None: + neg_code = self._ADS_N_AINCOM + + inpmux = (pos_code & 0xF0) | (neg_code & 0x0F) + self.write_single_register(self._REG_ADDR_INPMUX, inpmux) + + def wait_and_read_code16(self, *, timeout_ms: int = 200) -> tuple[int, Optional[int]]: + """ + Wait for DRDY falling edge then read conversion result. + + Args: + timeout_ms (int): Timeout in milliseconds to wait for DRDY. + Defaults to 200. + + Returns: + tuple[int, int | None]: Signed 16-bit conversion code and + optional status byte. + + Raises: + TimeoutError: If DRDY does not assert within timeout_ms. + """ + ok = self.wait_for_drdy_htol(timeout_ms) + if not ok: + raise TimeoutError(f"Timeout waiting for DRDY (>{timeout_ms} ms)") + + code16, status = self.read_converted_data(mode=self.ReadMode.DIRECT) + return code16, status + + def discard_settling_reads(self, n: int = 1, timeout_ms: int = 200) -> None: + """ + Discard a few reads after changing MUX to reduce charge-injection artifacts. + + Args: + n (int): Number of reads to discard. Defaults to 1. + timeout_ms (int): Timeout in milliseconds to wait for each DRDY. Defaults to 200. + """ + for _ in range(max(0, n)): + self.send_start() + _ = self.wait_and_read_code16(timeout_ms=timeout_ms) + + def code16_to_volts(self, code16: int) -> float: + """ + Convert a signed 16-bit ADC code to a voltage in volts. + + Uses the full-scale formula: Vin = code * (Vref / gain) / 32768. + + Args: + code16 (int): Signed 16-bit ADC code in the range [-32768, 32767]. + + Returns: + float: Corresponding input voltage in volts. + """ + return (code16 * (self._voltage_reference / float(self._pga_gain))) / 32768.0 + + # Functions transferred from VSO_CODEBASE_DEV repo multi_channel_read.py + def adc_configure_common( + self, + single_shot: bool = True, + filter_low_latency: bool = True, + vref_select_reg: int = _REF_DEFAULT, + enable_crc: bool = False, + enable_status_byte: bool = False, + ) -> None: + """ + Configure shared ADC registers (PGA, DATARATE, REF, SYS). + + Args: + single_shot (bool): If True, use single-shot conversion mode. Defaults to True. + filter_low_latency (bool): If True, enable the low-latency digital filter. Defaults to True. + vref_select_reg (int): REF register value selecting the voltage reference source. Defaults to _REF_DEFAULT. + enable_crc (bool): If True, enable CRC error checking on data reads. Defaults to False. + enable_status_byte (bool): If True, prepend a STATUS byte to each conversion result. Defaults to False. + + Raises: + ValueError: If pga_gain or data_rate is not a supported value. + """ + # --- PGA --- + if self._pga_gain == 1: + pga_reg = self._ADS_PGA_BYPASS | self._ADS_GAIN_1 + else: + gain_to_code = { + 1: self._ADS_GAIN_1, + 2: self._ADS_GAIN_2, + 4: self._ADS_GAIN_4, + 8: self._ADS_GAIN_8, + 16: self._ADS_GAIN_16, + 32: self._ADS_GAIN_32, + 64: self._ADS_GAIN_64, + 128: self._ADS_GAIN_128, + } + if self._pga_gain not in gain_to_code: + raise ValueError(f"Unsupported gain={self._pga_gain}. Choose from {sorted(gain_to_code.keys())}") + pga_reg = self._ADS_PGA_ENABLED | gain_to_code[self._pga_gain] + self.write_single_register(self._REG_ADDR_PGA, pga_reg) + + # --- DATARATE --- + rate_to_code = { + 2.5: self._ADS_DR_2_5, + 5: self._ADS_DR_5, + 10: self._ADS_DR_10, + 16: self._ADS_DR_16, + 20: self._ADS_DR_20, + 50: self._ADS_DR_50, + 60: self._ADS_DR_60, + 100: self._ADS_DR_100, + 200: self._ADS_DR_200, + 400: self._ADS_DR_400, + 800: self._ADS_DR_800, + 1000: self._ADS_DR_1000, + 2000: self._ADS_DR_2000, + 4000: self._ADS_DR_4000, + } + if self._data_rate not in rate_to_code: + raise ValueError(f"Unsupported frequency={self._data_rate}. Choose from {sorted(rate_to_code.keys())}") + data_rate_code = rate_to_code[self._data_rate] + convmode = self._ADS_CONVMODE_SS if single_shot else self._ADS_CONVMODE_CONT + ftype = self._ADS_FILTERTYPE_LL if filter_low_latency else 0x00 + datarate_reg = convmode | ftype | (data_rate_code & 0x0F) + self.write_single_register(self._REG_ADDR_DATARATE, datarate_reg) + + # REF + self.write_single_register(self._REG_ADDR_REF, vref_select_reg) + + # SYS (CRC + SENDSTAT) + sys_reg = self._ADS_SYS_MON_OFF + sys_reg |= self._ADS_CRC_ENABLE if enable_crc else self._ADS_CRC_DISABLE + sys_reg |= self._ADS_SENDSTATUS_ENABLE if enable_status_byte else self._ADS_SENDSTATUS_DISABLE + self.write_single_register(self._REG_ADDR_SYS, sys_reg) + + if enable_crc: + self.init_crc() + + +@dataclass +class ChannelConfig: + """ + For ADS114S0x: + + Defines how to read and post-process one ADC input. + """ + + name: str + ain_pos_code: int = ADS114S0x._ADS_P_AIN0 + ain_neg_code: int = ADS114S0x._ADS_N_AINCOM + postprocess: Optional[Callable[[float], float]] = None + units: str = "V" + + class ADS131M0x(ADCBase): """ Class for communication with the ADS131M0x family of ADC chips. @@ -66,7 +1162,7 @@ def __init__( voltage_reference: float = 1.2, gain_error: Optional[list[int]] = None, offline: bool = False, - ): + ) -> None: """ Initialize the ADS131M0x instance. @@ -154,12 +1250,12 @@ def update(self) -> None: Update the ADC data by reading the latest voltage values in millivolts. Attempts to read a maximum of 1000 times before throwing an error. """ - MAX_ATTEMPTS = 1000 + max_attempts = 1000 attempts = 0 while not self._ready_to_read(): sleep(0.001) attempts += 1 - if attempts > MAX_ATTEMPTS: + if attempts > max_attempts: raise RuntimeError( "Couldn't connect to the ADC, please ensure that the device is connected and powered on." ) @@ -171,8 +1267,6 @@ def calibrate(self) -> None: Perform offset and gain calibration on the ADC. """ self._offset_calibration() - # if self._gain_error is not None: - # self._gain_calibration() def read_register(self, address: int) -> int: """ @@ -288,7 +1382,7 @@ def _spi_message(self, msg: list[int]) -> Any: """Send SPI message to ADS131M0x. Args: - - msg (List[int]): message to be sent to the ADS131M0x separated into bytes. + msg (list[int]): message to be sent to the ADS131M0x separated into bytes. Returns: list[int]: The response from the device, representing the entire frame. """ @@ -302,12 +1396,12 @@ def _channel_enable(self, state: bool) -> None: Args: state (bool): If True, enables the channel clocks; if False, disables them. """ - OSR = (self._clock_freq / 2) / self._data_rate - OSR_reg = int(math.log2(OSR) - 7) + osr = (self._clock_freq / 2) / self._data_rate + osr_reg = int(math.log2(osr) - 7) self._ENABLE_CHANNELS_CLOCK &= ~(0b111 << 2) - self._ENABLE_CHANNELS_CLOCK |= OSR_reg << 2 + self._ENABLE_CHANNELS_CLOCK |= osr_reg << 2 self._DISABLE_CHANNELS_CLOCK &= ~(0b111 << 2) - self._DISABLE_CHANNELS_CLOCK |= OSR_reg << 2 + self._DISABLE_CHANNELS_CLOCK |= osr_reg << 2 if state is True: self.write_register(self._CLOCK_REG, self._ENABLE_CHANNELS_CLOCK) elif state is False: @@ -403,8 +1497,8 @@ def _ready_to_read(self) -> bool: def _read_data_millivolts(self) -> Any: """Returns channel readings in millivolts.""" self._data_counts = self._read_data_counts() - mV = 1000 * self._data_counts / 2 ** (self._RESOLUTION - 1) * self._voltage_reference - return mV + mv = 1000 * self._data_counts / 2 ** (self._RESOLUTION - 1) * self._voltage_reference + return mv def _read_data_counts(self) -> np.ndarray: """Returns channel readings in counts ranging from -2^23 -> 2^23-1""" diff --git a/pyproject.toml b/pyproject.toml index 6f01e87b..84ccf48e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -6,6 +6,7 @@ authors = [{ name = "Open-Source Leg", email = "opensourceleg@gmail.com" }] requires-python = ">=3.11,<4.0" readme = "README.md" dependencies = [ + "gpiozero>=2.0.1.post3", "numpy>=1.24.3,<2", "pandas>=2.2.3,<3", ] diff --git a/uv.lock b/uv.lock index 718fe31d..e393c6d5 100644 --- a/uv.lock +++ b/uv.lock @@ -17,7 +17,7 @@ dependencies = [ { name = "adafruit-pureio" }, { name = "binho-host-adapter" }, { name = "pyftdi" }, - { name = "sysv-ipc", marker = "platform_machine != 'mips' and sys_platform == 'linux'" }, + { name = "sysv-ipc" }, ] sdist = { url = "https://files.pythonhosted.org/packages/fd/59/786ca2acecaccf3a71d0e467d734c2726dc104de82d6e45d5c6400efc3a2/adafruit_blinka-8.64.0.tar.gz", hash = "sha256:a81c21ff57706e269fa4185d4ffe6784a27f57c3c4cfd91039c1957d5308205d", size = 267967, upload-time = "2025-08-18T16:51:05.735Z" } wheels = [ @@ -319,6 +319,18 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/d1/d6/3965ed04c63042e047cb6a3e6ed1a63a35087b6a609aa3a15ed8ac56c221/colorama-0.4.6-py2.py3-none-any.whl", hash = "sha256:4f1d9991f5acc0ca119f9d443620b77f9d6b33703e51011c16baf57afb285fc6", size = 25335, upload-time = "2022-10-25T02:36:20.889Z" }, ] +[[package]] +name = "colorzero" +version = "2.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "setuptools" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/b3/ca/688824a06e8c4d04c7d2fd2af2d8da27bed51af20ee5f094154e1d680334/colorzero-2.0.tar.gz", hash = "sha256:e7d5a5c26cd0dc37b164ebefc609f388de24f8593b659191e12d85f8f9d5eb58", size = 25382, upload-time = "2021-03-15T23:42:23.261Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/7e/a6/ddd0f130e44a7593ac6c55aa93f6e256d2270fd88e9d1b64ab7f22ab8fde/colorzero-2.0-py2.py3-none-any.whl", hash = "sha256:0e60d743a6b8071498a56465f7719c96a5e92928f858bab1be2a0d606c9aa0f8", size = 26573, upload-time = "2021-03-15T23:42:21.757Z" }, +] + [[package]] name = "coverage" version = "7.10.5" @@ -463,6 +475,18 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/f7/ec/67fbef5d497f86283db54c22eec6f6140243aae73265799baaaa19cd17fb/ghp_import-2.1.0-py3-none-any.whl", hash = "sha256:8337dd7b50877f163d4c0289bc1f1c7f127550241988d568c1db512c4324a619", size = 11034, upload-time = "2022-05-02T15:47:14.552Z" }, ] +[[package]] +name = "gpiozero" +version = "2.0.1.post3" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "colorzero" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/67/63/864a3c36842cf7b05808aadf906763a024a1b13f2a4c8144e3163de91255/gpiozero-2.0.1.post3.tar.gz", hash = "sha256:745feab6df463ac2e9de10c67e2dd9f396e668ba4e281e92381d6c460100a8f7", size = 168403, upload-time = "2026-07-27T10:03:32.628Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/45/cb/ec5917b454b2e82874769a348dc74cfcec5b447bf1061c827edea9702451/gpiozero-2.0.1.post3-py3-none-any.whl", hash = "sha256:d491734803a9bc6036602e6b8bbd73fb25c33da88de0fcd11a40e890b8bc2d3b", size = 151465, upload-time = "2026-07-27T10:03:31.195Z" }, +] + [[package]] name = "griffe" version = "1.13.0" @@ -917,6 +941,7 @@ name = "opensourceleg" version = "3.5.0" source = { editable = "." } dependencies = [ + { name = "gpiozero" }, { name = "numpy" }, { name = "pandas" }, ] @@ -980,6 +1005,7 @@ requires-dist = [ { name = "adafruit-circuitpython-lis3dh", marker = "platform_machine == 'aarch64' and sys_platform == 'linux' and extra == 'bno055'", specifier = ">=5.2.2,<6" }, { name = "board", marker = "extra == 'bno055'", specifier = "~=1.0" }, { name = "flexsea", marker = "extra == 'dephy'" }, + { name = "gpiozero", specifier = ">=2.0.1.post3" }, { name = "grpcio", marker = "extra == 'messaging'", specifier = ">=1.65.5,<2" }, { name = "grpcio-tools", marker = "extra == 'messaging'", specifier = ">=1.65.5,<2" }, { name = "moteus", marker = "extra == 'moteus'", specifier = ">=0.3.72,<0.4" }, @@ -1098,8 +1124,8 @@ resolution-markers = [ "python_full_version < '3.12'", ] dependencies = [ - { name = "python-dateutil", marker = "python_full_version < '3.12'" }, - { name = "pytzdata", marker = "python_full_version < '3.12'" }, + { name = "python-dateutil" }, + { name = "pytzdata" }, ] sdist = { url = "https://files.pythonhosted.org/packages/db/15/6e89ae7cde7907118769ed3d2481566d05b5fd362724025198bb95faf599/pendulum-2.1.2.tar.gz", hash = "sha256:b06a0ca1bfe41c990bbf0c029f0b6501a7f2ec4e38bfec730712015e8860f207", size = 81167, upload-time = "2020-07-24T18:17:03.724Z" } @@ -1111,8 +1137,8 @@ resolution-markers = [ "python_full_version >= '3.12'", ] dependencies = [ - { name = "python-dateutil", marker = "python_full_version >= '3.12'" }, - { name = "tzdata", marker = "python_full_version >= '3.12'" }, + { name = "python-dateutil" }, + { name = "tzdata" }, ] sdist = { url = "https://files.pythonhosted.org/packages/23/7c/009c12b86c7cc6c403aec80f8a4308598dfc5995e5c523a5491faaa3952e/pendulum-3.1.0.tar.gz", hash = "sha256:66f96303560f41d097bee7d2dc98ffca716fbb3a832c4b3062034c2d45865015", size = 85930, upload-time = "2025-04-19T14:30:01.675Z" } wheels = [ From b1803d72be6f9958a5fc21a0d3ce2435fc9343c0 Mon Sep 17 00:00:00 2001 From: Emily Date: Thu, 13 Aug 2026 14:52:55 -0400 Subject: [PATCH 03/17] test: add UnitTest for hall, encoderCounter, and adc sensor classes --- tests/test_sensors/test_adc.py | 641 ++++++++++++++++++++++ tests/test_sensors/test_encoderCounter.py | 149 +++++ tests/test_sensors/test_hall.py | 133 +++++ tests/test_sensors/test_sensors_base.py | 86 +++ 4 files changed, 1009 insertions(+) create mode 100644 tests/test_sensors/test_adc.py create mode 100644 tests/test_sensors/test_encoderCounter.py create mode 100644 tests/test_sensors/test_hall.py diff --git a/tests/test_sensors/test_adc.py b/tests/test_sensors/test_adc.py new file mode 100644 index 00000000..af1fc293 --- /dev/null +++ b/tests/test_sensors/test_adc.py @@ -0,0 +1,641 @@ +import sys +from unittest.mock import MagicMock, Mock, patch + +import pytest + +from opensourceleg.sensors import adc as adc_module +from opensourceleg.sensors.adc import ADS114S0x, ChannelConfig + + +# inmport fake spidev +@pytest.fixture(autouse=True) +def fake_spidev(monkeypatch): + module = MagicMock() + monkeypatch.setitem(sys.modules, "spidev", module) + return module + + +@pytest.fixture(autouse=True) +def reset_class_state(): + """ + _register_map and _crc_lookup_table are CLASS attributes, so mutations leak + between tests (and between real instances -- see notes). Snapshot/restore. + """ + crc = list(ADS114S0x._crc_lookup_table) + initialized = ADS114S0x._initialized + yield + ADS114S0x._crc_lookup_table[:] = crc + ADS114S0x._initialized = initialized + + +@pytest.fixture +def mock_spi(): + spi = Mock() + spi.xfer2 = Mock(return_value=[0, 0, 0]) + return spi + + +@pytest.fixture +def adc(mock_spi): + """A constructed ADS114S0x with SPI and DRDY replaced by mocks.""" + with patch.object(adc_module, "DigitalInputDevice") as mock_drdy: + device = ADS114S0x(tag="TestADC", spi_bus=0, spi_cs=0, data_rate=1000, pga_gain=1, drdy=16) + device._spi = mock_spi + device._drdy_mock = mock_drdy + return device + + +# Construction / basic properties +def test_init_defaults(mock_spi): + with patch.object(adc_module, "DigitalInputDevice") as mock_drdy: + device = ADS114S0x() + + assert device.tag == "ADS114S08" + assert device._spi_bus == 0 + assert device._spi_cs == 0 + assert device._data_rate == 400 + assert device._pga_gain == 1 + assert device._voltage_reference == pytest.approx(2.5) + assert device._streaming is False + assert device._channels == {} + mock_drdy.assert_called_once_with(16, pull_up=False) + + +def test_repr(adc): + assert repr(adc) == "ADS114S0x" + + +def test_is_streaming_reflects_device_state(adc): + assert adc.is_streaming is False + adc._set_device_state(1) + assert adc.is_streaming is True + adc._set_device_state(0) + assert adc.is_streaming is False + + +def test_set_device_state_ignores_unknown_values(adc): + adc._set_device_state(1) + adc._set_device_state(7) # not 0 or 1 -> no change + assert adc.is_streaming is True + + +def test_init_exits_when_spidev_missing(monkeypatch): + monkeypatch.delitem(sys.modules, "spidev", raising=False) + real_import = __builtins__["__import__"] if isinstance(__builtins__, dict) else __builtins__.__import__ + + def blocked_import(name, *args, **kwargs): + if name == "spidev": + raise ImportError("no spidev") + return real_import(name, *args, **kwargs) + + monkeypatch.setattr("builtins.__import__", blocked_import) + with patch.object(adc_module, "DigitalInputDevice"), pytest.raises(SystemExit): + ADS114S0x(offline=False) + + +# Register map helpers +def test_restore_register_defaults(adc): + adc._register_map[:] = [0xFF] * ADS114S0x._NUM_REGISTERS + adc.restore_register_defaults() + + assert adc.get_register_value(ADS114S0x._REG_ADDR_ID) == ADS114S0x._ID_DEFAULT + assert adc.get_register_value(ADS114S0x._REG_ADDR_STATUS) == ADS114S0x._STATUS_DEFAULT + assert adc.get_register_value(ADS114S0x._REG_ADDR_INPMUX) == ADS114S0x._INPMUX_DEFAULT + assert adc.get_register_value(ADS114S0x._REG_ADDR_PGA) == ADS114S0x._PGA_DEFAULT + assert adc.get_register_value(ADS114S0x._REG_ADDR_DATARATE) == ADS114S0x._DATARATE_DEFAULT + assert adc.get_register_value(ADS114S0x._REG_ADDR_REF) == ADS114S0x._REF_DEFAULT + assert adc.get_register_value(ADS114S0x._REG_ADDR_FSCAL1) == ADS114S0x._FSCAL1_DEFAULT + assert adc.get_register_value(ADS114S0x._REG_ADDR_GPIOCON) == ADS114S0x._GPIOCON_DEFAULT + + +def test_get_register_value_out_of_range(adc): + with pytest.raises(ValueError): + adc.get_register_value(ADS114S0x._NUM_REGISTERS + 1) + + +def test_is_sendstat_and_is_crc_set(adc): + adc._register_map[ADS114S0x._REG_ADDR_SYS] = 0x00 + assert adc.is_sendstat_set() is False + assert adc.is_crc_set() is False + + adc._register_map[ADS114S0x._REG_ADDR_SYS] = ADS114S0x._ADS_SENDSTATUS_MASK + assert adc.is_sendstat_set() is True + assert adc.is_crc_set() is False + + adc._register_map[ADS114S0x._REG_ADDR_SYS] = ADS114S0x._ADS_CRC_MASK + assert adc.is_sendstat_set() is False + assert adc.is_crc_set() is True + + +# Single / multiple register reads and writes +def test_read_single_register(adc, mock_spi): + mock_spi.xfer2.return_value = [0x00, 0x00, 0x42] + + value = adc.read_single_register(ADS114S0x._REG_ADDR_INPMUX) + + assert value == 0x42 + assert adc.get_register_value(ADS114S0x._REG_ADDR_INPMUX) == 0x42 + mock_spi.xfer2.assert_called_once_with([ADS114S0x._OPCODE_RREG | ADS114S0x._REG_ADDR_INPMUX, 0, 0]) + + +def test_read_single_register_out_of_range(adc): + with pytest.raises(ValueError): + adc.read_single_register(ADS114S0x._NUM_REGISTERS + 1) + + +def test_write_single_register(adc, mock_spi): + adc.write_single_register(ADS114S0x._REG_ADDR_PGA, 0x1AB) # >8 bits, should be masked + + assert adc.get_register_value(ADS114S0x._REG_ADDR_PGA) == 0xAB + mock_spi.xfer2.assert_called_once_with([ADS114S0x._OPCODE_WREG | ADS114S0x._REG_ADDR_PGA, 0, 0xAB]) + + +def test_write_single_register_out_of_range(adc): + with pytest.raises(ValueError): + adc.write_single_register(ADS114S0x._NUM_REGISTERS + 1, 0x00) + + +def test_read_multiple_registers(adc, mock_spi): + # 2 command bytes then 3 data bytes + mock_spi.xfer2.return_value = [0x00, 0x00, 0x11, 0x22, 0x33] + + adc.read_multiple_registers(start_address=0x02, count=3) + + assert adc.get_register_value(0x02) == 0x11 + assert adc.get_register_value(0x03) == 0x22 + assert adc.get_register_value(0x04) == 0x33 + mock_spi.xfer2.assert_called_once_with([ADS114S0x._OPCODE_RREG | 0x02, 2, 0, 0, 0]) + + +def test_read_multiple_registers_out_of_range(adc): + with pytest.raises(ValueError): + adc.read_multiple_registers(start_address=0x10, count=10) + + +def test_write_multiple_registers(adc, mock_spi): + adc.write_multiple_registers(start_address=0x02, count=3, reg_data=[0x11, 0x22, 0x133]) + + assert adc.get_register_value(0x02) == 0x11 + assert adc.get_register_value(0x03) == 0x22 + assert adc.get_register_value(0x04) == 0x33 + mock_spi.xfer2.assert_called_once_with([ADS114S0x._OPCODE_WREG | 0x02, 2, 0x11, 0x22, 0x33]) + + +def test_write_multiple_registers_out_of_range(adc): + with pytest.raises(ValueError): + adc.write_multiple_registers(start_address=0x10, count=10, reg_data=[0] * 10) + + +def test_write_multiple_registers_none_data(adc): + with pytest.raises(ValueError): + adc.write_multiple_registers(start_address=0x00, count=1, reg_data=None) + + +def test_register_map_is_isolated_between_instances(mock_spi): + with patch.object(adc_module, "DigitalInputDevice"): + a = ADS114S0x(tag="A") + b = ADS114S0x(tag="B") + a._spi = mock_spi + a.write_single_register(ADS114S0x._REG_ADDR_PGA, 0x07) + assert b.get_register_value(ADS114S0x._REG_ADDR_PGA) == 0x00 + + +# Commands +@pytest.mark.parametrize("opcode", [ADS114S0x._OPCODE_RREG, ADS114S0x._OPCODE_WREG]) +def test_send_command_rejects_register_opcodes(adc, opcode): + with pytest.raises(ValueError): + adc.send_command(opcode) + + +def test_send_command_writes_byte(adc, mock_spi): + mock_spi.xfer2.return_value = [0x7F] + adc.send_command(ADS114S0x._OPCODE_START) + mock_spi.xfer2.assert_called_once_with([ADS114S0x._OPCODE_START]) + + +def test_send_command_reset_restores_defaults(adc, mock_spi): + mock_spi.xfer2.return_value = [0x00] + adc._register_map[:] = [0xFF] * ADS114S0x._NUM_REGISTERS + + with patch.object(adc, "delay_us") as mock_delay: + adc.send_command(ADS114S0x._OPCODE_RESET) + + mock_delay.assert_called_once_with(ADS114S0x._DELAY_4096TCLK) + assert adc.get_register_value(ADS114S0x._REG_ADDR_STATUS) == ADS114S0x._STATUS_DEFAULT + + +@pytest.mark.parametrize( + ("method", "opcode"), + [ + ("send_start", ADS114S0x._OPCODE_START), + ("send_stop", ADS114S0x._OPCODE_STOP), + ("send_wakeup", ADS114S0x._OPCODE_WAKEUP), + ("send_powerdown", ADS114S0x._OPCODE_POWERDOWN), + ("reset", ADS114S0x._OPCODE_RESET), + ], +) +def test_command_wrappers(adc, method, opcode): + with patch.object(adc, "send_command") as mock_send: + getattr(adc, method)() + mock_send.assert_called_once_with(opcode) + + +def test_start_conversions_wakes_then_starts(adc): + with patch.object(adc, "send_wakeup") as wake, patch.object(adc, "send_start") as start: + adc.start_conversions() + wake.assert_called_once() + start.assert_called_once() + + +# SPI plumbing +def test_init_spi_configures_port(adc, mock_spi): + adc.init_spi() + mock_spi.open.assert_called_once_with(0, 0) + assert mock_spi.max_speed_hz == ADS114S0x._SPI_SPEED + assert mock_spi.mode == 0b01 + assert mock_spi.bits_per_word == 8 + + +def test_init_spi_raises_without_spi(adc): + adc._spi = None + with pytest.raises(RuntimeError): + adc.init_spi() + + +def test_spi_send_receive_arrays_truncates_to_byte_length(adc, mock_spi): + mock_spi.xfer2.return_value = [1, 2] + result = adc.spi_send_receive_arrays([1, 2, 3, 4], 2) + mock_spi.xfer2.assert_called_once_with([1, 2]) + assert result == [1, 2] + + +def test_spi_send_receive_arrays_raises_without_spi(adc): + adc._spi = None + with pytest.raises(RuntimeError): + adc.spi_send_receive_arrays([0], 1) + + +def test_spi_send_receive_byte_masks_input(adc, mock_spi): + mock_spi.xfer2.return_value = [0x5A] + assert adc.spi_send_receive_byte(0x1FF) == 0x5A + mock_spi.xfer2.assert_called_once_with([0xFF]) + + +def test_spi_send_receive_byte_raises_without_spi(adc): + adc._spi = None + with pytest.raises(RuntimeError): + adc.spi_send_receive_byte(0x00) + + +def test_cleanup_closes_and_clears_spi(adc, mock_spi): + adc.cleanup() + mock_spi.close.assert_called_once() + assert adc._spi is None + adc.cleanup() # idempotent + + +def test_delay_us(adc): + with patch.object(adc_module, "sleep") as mock_sleep: + adc.delay_us(2500) + mock_sleep.assert_called_once_with(pytest.approx(0.0025)) + + +def test_wait_for_drdy_htol_sleeps_one_conversion_period(adc): + with patch.object(adc_module, "sleep") as mock_sleep: + assert adc.wait_for_drdy_htol(timeout_ms=200) is True + mock_sleep.assert_called_once_with(pytest.approx(1.5 / adc._data_rate)) + + +# start / stop / update +def test_start_sequence(adc): + with ( + patch.object(adc, "init_spi") as init_spi, + patch.object(adc, "delay_us") as delay, + patch.object(adc, "reset") as reset, + patch.object(adc, "restore_register_defaults") as restore, + patch.object(adc, "write_single_register") as write, + ): + adc.start() + + init_spi.assert_called_once() + delay.assert_called_once_with(ADS114S0x._DELAY_2p2MS) + reset.assert_called_once() + restore.assert_called_once() + write.assert_called_once_with(ADS114S0x._REG_ADDR_STATUS, 0x00) + assert adc.is_streaming is True + + +def test_stop_sequence(adc): + adc._set_device_state(1) + with patch.object(adc, "send_stop") as stop, patch.object(adc, "cleanup") as cleanup: + adc.stop() + stop.assert_called_once() + cleanup.assert_called_once() + + +def test_update_stores_millivolts(adc): + assert adc.data is None + with ( + patch.object(adc, "_ready_to_read", return_value=True), + patch.object(adc, "_read_data_millivolts", return_value=[1.0, 2.0]), + ): + adc.update() + assert adc.data == [1.0, 2.0] + + +def test_update_raises_when_never_ready(adc): + with ( + patch.object(adc, "_ready_to_read", return_value=False), + patch.object(adc_module, "sleep"), + pytest.raises(RuntimeError), + ): + adc.update() + + +def test_ready_to_read(adc): + with patch.object(adc, "read_single_register", return_value=0x00): + assert adc._ready_to_read() is True + with patch.object(adc, "read_single_register", return_value=ADS114S0x._ADS_nRDY_MASK): + assert adc._ready_to_read() is False + + +def test_read_data_millivolts_without_channels(adc): + assert adc._read_data_millivolts() is None + + +def test_read_data_millivolts_reads_each_channel(adc): + adc._channels = { + "a": ChannelConfig(name="a", ain_pos_code=ADS114S0x._ADS_P_AIN0), + "b": ChannelConfig(name="b", ain_pos_code=ADS114S0x._ADS_P_AIN3, postprocess=lambda mv: mv * 2), + } + + with ( + patch.object(adc, "set_mux_single_ended") as set_mux, + patch.object(adc, "discard_settling_reads") as discard, + patch.object(adc, "start_conversions") as start_conv, + patch.object(adc, "wait_and_read_code16", return_value=(3277, None)), + ): + row = adc._read_data_millivolts() + + expected_mv = (3277 * 2.5 / 32768.0) * 1000 + assert row[0] == pytest.approx(expected_mv) + assert row[1] == pytest.approx(expected_mv * 2) # postprocess applied + assert set_mux.call_count == 2 + assert discard.call_count == 2 + assert start_conv.call_count == 2 + + +# Conversion data parsing +def _configure_plain_read(adc): + """No STATUS byte, no CRC.""" + adc._register_map[ADS114S0x._REG_ADDR_SYS] = 0x00 + + +def test_read_converted_data_positive(adc, mock_spi): + _configure_plain_read(adc) + mock_spi.xfer2.return_value = [0x12, 0x34, 0x00] + + code16, status = adc.read_converted_data() + + assert code16 == 0x1234 + assert status is None + mock_spi.xfer2.assert_called_once_with([0, 0, 0]) + + +def test_read_converted_data_negative_sign_extends(adc, mock_spi): + _configure_plain_read(adc) + mock_spi.xfer2.return_value = [0xFF, 0xFF, 0x00] + + code16, _ = adc.read_converted_data() + + assert code16 == -1 + + +def test_read_converted_data_full_scale(adc, mock_spi): + _configure_plain_read(adc) + mock_spi.xfer2.return_value = [0x80, 0x00, 0x00] + + code16, _ = adc.read_converted_data() + + assert code16 == -32768 + + +def test_read_converted_data_command_mode_sends_rdata(adc, mock_spi): + _configure_plain_read(adc) + mock_spi.xfer2.return_value = [0x00, 0x12, 0x34, 0x00] + + code16, status = adc.read_converted_data(mode=ADS114S0x.ReadMode.COMMAND) + + assert mock_spi.xfer2.call_args[0][0][0] == ADS114S0x._OPCODE_RDATA + assert code16 == 0x1234 + assert status is None + + +def test_read_converted_data_with_status_byte(adc, mock_spi): + adc._register_map[ADS114S0x._REG_ADDR_SYS] = ADS114S0x._ADS_SENDSTATUS_MASK + mock_spi.xfer2.return_value = [0x80, 0x12, 0x34, 0x00] + + code16, status = adc.read_converted_data() + + assert status == 0x80 + assert code16 == 0x1234 + + +def test_read_converted_data_crc_error_raises(adc, mock_spi): + adc._register_map[ADS114S0x._REG_ADDR_SYS] = ADS114S0x._ADS_CRC_MASK + mock_spi.xfer2.return_value = [0x12, 0x34, 0x00, 0xFF] + + with patch.object(adc, "get_crc", return_value=1), pytest.raises(ValueError): + adc.read_converted_data() + + +def test_wait_and_read_code16_success(adc): + with ( + patch.object(adc, "wait_for_drdy_htol", return_value=True), + patch.object(adc, "read_converted_data", return_value=(1234, None)) as read, + ): + code16, status = adc.wait_and_read_code16(timeout_ms=50) + + assert (code16, status) == (1234, None) + read.assert_called_once_with(mode=ADS114S0x.ReadMode.DIRECT) + + +def test_wait_and_read_code16_timeout(adc): + with patch.object(adc, "wait_for_drdy_htol", return_value=False), pytest.raises(TimeoutError): + adc.wait_and_read_code16(timeout_ms=50) + + +def test_discard_settling_reads(adc): + with patch.object(adc, "send_start") as start, patch.object(adc, "wait_and_read_code16") as read: + adc.discard_settling_reads(n=3, timeout_ms=100) + assert start.call_count == 3 + assert read.call_count == 3 + + +def test_discard_settling_reads_negative_n_is_noop(adc): + with patch.object(adc, "send_start") as start: + adc.discard_settling_reads(n=-5) + start.assert_not_called() + + +# Voltage conversion +@pytest.mark.parametrize( + ("code16", "gain", "expected"), + [ + (0, 1, 0.0), + (32767, 1, 32767 * 2.5 / 32768.0), + (-32768, 1, -2.5), + (16384, 1, 1.25), + (16384, 2, 0.625), + (16384, 128, 1.25 / 128), + ], +) +def test_code16_to_volts(adc, code16, gain, expected): + adc._pga_gain = gain + assert adc.code16_to_volts(code16) == pytest.approx(expected) + + +def test_code16_to_volts_uses_voltage_reference(adc): + adc._voltage_reference = 5.0 + assert adc.code16_to_volts(16384) == pytest.approx(2.5) + + +# MUX +def test_set_mux_single_ended_defaults_to_aincom(adc): + with patch.object(adc, "write_single_register") as write: + adc.set_mux_single_ended(ADS114S0x._ADS_P_AIN3) + write.assert_called_once_with(ADS114S0x._REG_ADDR_INPMUX, 0x30 | ADS114S0x._ADS_N_AINCOM) + + +def test_set_mux_single_ended_with_negative_code(adc): + with patch.object(adc, "write_single_register") as write: + adc.set_mux_single_ended(ADS114S0x._ADS_P_AIN5, ADS114S0x._ADS_N_AIN2) + write.assert_called_once_with(ADS114S0x._REG_ADDR_INPMUX, 0x52) + + +# CRC +def test_calculate_crc_of_zero_is_zero(adc): + assert adc._calculate_crc([0x00], 1) == 0x00 + + +def test_lookup_matches_calculate(adc): + adc.init_crc() + for message in ([0x01], [0xFF, 0x00], [0x12, 0x34, 0x56], [0xDE, 0xAD, 0xBE, 0xEF]): + assert adc._lookup_crc(message, len(message)) == adc._calculate_crc(message, len(message)) + + +def test_get_crc_detects_no_error_when_crc_appended(adc): + message = [0x12, 0x34, 0x56] + crc = adc.get_crc(message, len(message)) + assert adc.get_crc([*message, crc], len(message) + 1) == 0 + + +def test_get_crc_detects_corruption(adc): + message = [0x12, 0x34, 0x56] + crc = adc.get_crc(message, len(message)) + corrupted = [0x12, 0x35, 0x56, crc] + assert adc.get_crc(corrupted, 4) != 0 + + +def test_init_crc_builds_lookup_table(adc): + assert adc._initialized is False # reads through to the class attr + adc.init_crc() + assert adc._initialized is True + assert "_initialized" in adc.__dict__ # instance attr now shadows the class one + assert len(ADS114S0x._crc_lookup_table) == 256 + assert ADS114S0x._crc_lookup_table[0] == 0x00 + + +def test_get_crc_initializes_table_lazily(adc): + ADS114S0x._initialized = False + ADS114S0x._crc_lookup_table[:] = [0] * 256 + result = adc.get_crc([0x12, 0x34], 2) + assert result == adc._calculate_crc([0x12, 0x34], 2) + + +# adc_configure_common +def test_adc_configure_common_bypasses_pga_at_gain_1(adc): + adc._pga_gain = 1 + adc._data_rate = 1000 + with patch.object(adc, "write_single_register") as write: + adc.adc_configure_common() + + write.assert_any_call(ADS114S0x._REG_ADDR_PGA, ADS114S0x._ADS_PGA_BYPASS | ADS114S0x._ADS_GAIN_1) + + +def test_adc_configure_common_enables_pga(adc): + adc._pga_gain = 32 + adc._data_rate = 1000 + with patch.object(adc, "write_single_register") as write: + adc.adc_configure_common() + + write.assert_any_call(ADS114S0x._REG_ADDR_PGA, ADS114S0x._ADS_PGA_ENABLED | ADS114S0x._ADS_GAIN_32) + + +def test_adc_configure_common_datarate_register(adc): + adc._data_rate = 1000 + with patch.object(adc, "write_single_register") as write: + adc.adc_configure_common(single_shot=True, filter_low_latency=True) + + expected = ADS114S0x._ADS_CONVMODE_SS | ADS114S0x._ADS_FILTERTYPE_LL | ADS114S0x._ADS_DR_1000 + write.assert_any_call(ADS114S0x._REG_ADDR_DATARATE, expected) + + +def test_adc_configure_common_continuous_mode(adc): + adc._data_rate = 100 + with patch.object(adc, "write_single_register") as write: + adc.adc_configure_common(single_shot=False, filter_low_latency=False) + + write.assert_any_call(ADS114S0x._REG_ADDR_DATARATE, ADS114S0x._ADS_CONVMODE_CONT | ADS114S0x._ADS_DR_100) + + +def test_adc_configure_common_sys_register(adc): + adc._data_rate = 1000 + with patch.object(adc, "write_single_register") as write, patch.object(adc, "init_crc") as init_crc: + adc.adc_configure_common(enable_crc=True, enable_status_byte=True) + + expected = ADS114S0x._ADS_SYS_MON_OFF | ADS114S0x._ADS_CRC_ENABLE | ADS114S0x._ADS_SENDSTATUS_ENABLE + write.assert_any_call(ADS114S0x._REG_ADDR_SYS, expected) + init_crc.assert_called_once() + + +def test_adc_configure_common_no_crc_skips_init(adc): + adc._data_rate = 1000 + with patch.object(adc, "write_single_register"), patch.object(adc, "init_crc") as init_crc: + adc.adc_configure_common(enable_crc=False) + init_crc.assert_not_called() + + +def test_adc_configure_common_rejects_bad_gain(adc): + adc._pga_gain = 3 + adc._data_rate = 1000 + with patch.object(adc, "write_single_register"), pytest.raises(ValueError, match="Unsupported gain"): + adc.adc_configure_common() + + +def test_adc_configure_common_rejects_bad_data_rate(adc): + adc._data_rate = 500 # NOTE: this is the constructor default -- see notes + with patch.object(adc, "write_single_register"), pytest.raises(ValueError, match="Unsupported frequency"): + adc.adc_configure_common() + + +def test_adc_configure_common_writes_ref_register(adc): + adc._data_rate = 1000 + with patch.object(adc, "write_single_register") as write: + adc.adc_configure_common(vref_select_reg=ADS114S0x._ADS_REFSEL_INT) + write.assert_any_call(ADS114S0x._REG_ADDR_REF, ADS114S0x._ADS_REFSEL_INT) + + +# ChannelConfig +def test_channel_config_defaults(): + ch = ChannelConfig(name="knee") + assert ch.name == "knee" + assert ch.ain_pos_code == ADS114S0x._ADS_P_AIN0 + assert ch.ain_neg_code == ADS114S0x._ADS_N_AINCOM + assert ch.postprocess is None + assert ch.units == "V" + + +def test_channel_config_postprocess_is_callable(): + ch = ChannelConfig(name="scaled", postprocess=lambda mv: mv / 10) + assert ch.postprocess(100.0) == pytest.approx(10.0) diff --git a/tests/test_sensors/test_encoderCounter.py b/tests/test_sensors/test_encoderCounter.py new file mode 100644 index 00000000..a57d40fa --- /dev/null +++ b/tests/test_sensors/test_encoderCounter.py @@ -0,0 +1,149 @@ +from unittest.mock import MagicMock, patch + +import pytest + +from opensourceleg.sensors.encoderCounter import LS7366R + + +@pytest.fixture +def mock_spi(): + """ + Patch spidev.SpiDev and the init-time sleep so an LS7366R can be built and + exercised without any real SPI bus. Yields the mocked SpiDev instance. + """ + with ( + patch("opensourceleg.sensors.encoderCounter.spidev.SpiDev") as mock_spidev_cls, + patch("opensourceleg.sensors.encoderCounter.sleep"), + ): + spi_instance = MagicMock() + mock_spidev_cls.return_value = spi_instance + yield spi_instance + + +@pytest.fixture +def encoder_counter(mock_spi): + """A default LS7366R (4-byte mode) backed by the mocked SPI device.""" + return LS7366R() + + +# test initialize/configuration +def test_ls7366r_init_defaults(encoder_counter: LS7366R, mock_spi: MagicMock): + assert encoder_counter.counter_size == 4 + assert encoder_counter.max_val == 4294967295 + # SPI device opened on the default bus/chip-select and clocked correctly. + mock_spi.open.assert_called_once_with(0, 0) + assert mock_spi.max_speed_hz == 1000000 + + +def test_ls7366r_init_custom_params(mock_spi: MagicMock): + enc = LS7366R(csx=1, clk=500000, byte_mode=2, spi_bus=0, max_val=65535) + assert enc.counter_size == 2 + assert enc.max_val == 65535 + # spi.open is called as open(spi_bus, csx). + mock_spi.open.assert_called_once_with(0, 1) + assert mock_spi.max_speed_hz == 500000 + + +def test_ls7366r_init_configures_modes(mock_spi: MagicMock): + LS7366R(byte_mode=4) + # Mode 0: quadrature count mode write. + mock_spi.xfer2.assert_any_call([LS7366R.WRITE_MODE0, LS7366R.QUADRATURE_COUNT_MODE]) + # Mode 1: byte-mode write (4-byte -> FOURBYTE_COUNTER == 0x00). + mock_spi.xfer2.assert_any_call([LS7366R.WRITE_MODE1, LS7366R.CounterConfig.FOURBYTE_COUNTER]) + + +# test clear_counter +def test_clear_counter(encoder_counter: LS7366R, mock_spi: MagicMock): + mock_spi.reset_mock() # discard the init-time transactions + result = encoder_counter.clear_counter() + assert result == "[DONE]" + mock_spi.xfer2.assert_called_once_with([LS7366R.CLEAR_COUNTER]) + + +# test clear_status +def test_clear_status(encoder_counter: LS7366R, mock_spi: MagicMock): + mock_spi.reset_mock() + result = encoder_counter.clear_status() + assert result == "[DONE]" + mock_spi.xfer2.assert_called_once_with([LS7366R.CLEAR_STATUS]) + + +# test read_counter +def test_read_counter_positive(encoder_counter: LS7366R, mock_spi: MagicMock): + # 4-byte payload -> 0x0000012C == 300; leading count byte != 255 -> positive. + mock_spi.xfer2.return_value = [0x00, 0x00, 0x00, 0x01, 0x2C] + assert encoder_counter.read_counter() == 300 + assert encoder_counter.encoder_count == 300 + + +def test_read_counter_negative(encoder_counter: LS7366R, mock_spi: MagicMock): + # Leading count byte == 255 triggers the signed wrap: 0xFFFFFFFF -> -1. + mock_spi.xfer2.return_value = [0x00, 0xFF, 0xFF, 0xFF, 0xFF] + assert encoder_counter.read_counter() == -1 + + +def test_read_counter_two_byte_mode(mock_spi: MagicMock): + enc = LS7366R(byte_mode=2) + # 2-byte payload -> 0x012C == 300. + mock_spi.xfer2.return_value = [0x00, 0x01, 0x2C] + assert enc.read_counter() == 300 + + +def test_read_counter_transaction_length(encoder_counter: LS7366R, mock_spi: MagicMock): + mock_spi.reset_mock() + mock_spi.xfer2.return_value = [0x00, 0x00, 0x00, 0x00, 0x00] + encoder_counter.read_counter() + # READ_COUNTER command byte followed by counter_size zero placeholders. + mock_spi.xfer2.assert_called_once_with([LS7366R.READ_COUNTER, 0, 0, 0, 0]) + + +# test read_status +def test_read_status(encoder_counter: LS7366R, mock_spi: MagicMock): + mock_spi.reset_mock() + mock_spi.xfer2.return_value = [0x00, 0x42] + assert encoder_counter.read_status() == 0x42 + mock_spi.xfer2.assert_called_once_with([LS7366R.READ_STATUS, 0xFF]) + + +# test start +def test_start_is_noop(encoder_counter: LS7366R): + assert encoder_counter.start() is None + + +def test_update_reads_counter(encoder_counter: LS7366R, mock_spi: MagicMock): + mock_spi.xfer2.return_value = [0x00, 0x00, 0x00, 0x00, 0x2A] + encoder_counter.update() + assert encoder_counter.encoder_count == 0x2A + + +def test_count_property(encoder_counter: LS7366R, mock_spi: MagicMock): + mock_spi.xfer2.return_value = [0x00, 0x00, 0x00, 0x01, 0x00] + assert encoder_counter.count == 256 + + +def test_close(encoder_counter: LS7366R, mock_spi: MagicMock): + encoder_counter.close() + mock_spi.close.assert_called_once() + assert encoder_counter.spi is None + + +def test_stop_closes_connection(encoder_counter: LS7366R, mock_spi: MagicMock): + encoder_counter.stop() + mock_spi.close.assert_called_once() + assert encoder_counter.spi is None + + +# Unsupported properties +def test_data_not_implemented(encoder_counter: LS7366R): + with pytest.raises(NotImplementedError): + _ = encoder_counter.data + + +def test_is_streaming_not_implemented(encoder_counter: LS7366R): + with pytest.raises(NotImplementedError): + _ = encoder_counter.is_streaming + + +# test repr (inherited from Encoder_counterCounterBase) +def test_repr(encoder_counter: LS7366R): + assert repr(encoder_counter) == "EncoderCounterBase" diff --git a/tests/test_sensors/test_hall.py b/tests/test_sensors/test_hall.py new file mode 100644 index 00000000..b8e83bee --- /dev/null +++ b/tests/test_sensors/test_hall.py @@ -0,0 +1,133 @@ +from unittest.mock import patch + +import pytest + +from opensourceleg.sensors import hall + + +@pytest.fixture +def sensor(): + return hall.DRV5056() + + +# Test DRV5056 init +def test_DRV5056_init(): + sensor = hall.DRV5056() + assert sensor.tag == "DRV5056A1" + assert sensor._sensor_num == "A1" + assert sensor._t_a == 23 + assert sensor._supply_voltage == 5 + + custom = hall.DRV5056(tag="MyHall", sensor_num="A3", t_a=25, supply_voltage=3.3) + assert custom.tag == "MyHall" + assert custom._sensor_num == "A3" + assert custom._t_a == 25 + assert custom._supply_voltage == 3.3 + + # offline mode is not supported and should exit + with pytest.raises(SystemExit): + hall.DRV5056(offline=True) + + +# Test DRV5056 repr +def test_DRV5056_repr(sensor: hall.DRV5056): + assert sensor.__repr__() == "DRV5056" + + +# Test DRV5056 configure with default 5V supply +def test_DRV5056_configure_default(sensor: hall.DRV5056): + sensor.configure() + assert sensor.base_sensitivity == 200 + assert sensor.lower_volt_sensitivity == pytest.approx(0.6 * 200) + assert sensor.base_range == 20 + assert sensor.lower_volt_range == 19 + assert sensor._s_tc == 0.0012 + assert sensor.range == sensor.base_range + assert sensor._sensitivity == sensor.base_sensitivity + + +# Test DRV5056 configure with 3.3V supply +def test_DRV5056_configure_3_3V(): + sensor = hall.DRV5056(sensor_num="A1", supply_voltage=3.3) + sensor.configure() + assert sensor.range == sensor.lower_volt_range + assert sensor._sensitivity == pytest.approx(sensor.lower_volt_sensitivity) + + +# Test DRV5056 configure with a supply voltage in the 4.5-5.5V range but not exactly 5V +def test_DRV5056_configure_near_5V(): + sensor = hall.DRV5056(sensor_num="A1", supply_voltage=4.8) + sensor.configure() + assert sensor.range == sensor.base_range + assert sensor._sensitivity == pytest.approx(sensor.base_sensitivity * 4.8 / 5) + + +# Test DRV5056 configure with a supply voltage in the 3-3.6V range but not exactly 3.3V +def test_DRV5056_configure_near_3_3V(): + sensor = hall.DRV5056(sensor_num="A1", supply_voltage=3.5) + sensor.configure() + assert sensor.range == sensor.lower_volt_range + assert sensor._sensitivity == pytest.approx(sensor.lower_volt_sensitivity * 3.5 / 3.3) + + +# Test DRV5056 configure with an out-of-range supply voltage +def test_DRV5056_configure_voltage_out_of_range(): + sensor = hall.DRV5056(sensor_num="A1", supply_voltage=4.0) + with pytest.raises(ValueError): + sensor.configure() + + +# Test DRV5056 configure with an unsupported sensor number +def test_DRV5056_configure_invalid_sensor_num(): + sensor = hall.DRV5056(sensor_num="B1") + with pytest.raises(ValueError): + sensor.configure() + + +# Test DRV5056 start and stop +def test_DRV5056_start_stop(sensor: hall.DRV5056): + sensor.start() + assert sensor.is_streaming is True + + sensor.stop() + assert sensor.is_streaming is False + + +# Test DRV5056 voltage property +def test_DRV5056_voltage(sensor: hall.DRV5056): + assert sensor.voltage == 0.0 + + +# Test DRV5056 update +def test_DRV5056_update(sensor: hall.DRV5056): + sensor.configure() + sensor.update() + + expected = (sensor.voltage * sensor._V_TO_MV - sensor._QUIESCENT_OFFSET) / ( + sensor._sensitivity * (1 + (sensor._s_tc * (sensor._t_a - 25))) + ) + assert sensor.field_strength == pytest.approx(expected) + + +# Test DRV5056 update logs an error when the field strength reaches the sensor's range +@patch("opensourceleg.logging.logger.LOGGER.error") +def test_DRV5056_update_out_of_range_logs_error(mock_error, sensor: hall.DRV5056): + sensor.configure() + sensor.update() + sensor.range = sensor.field_strength + + sensor.update() + mock_error.assert_called_once() + + +# Test DRV5056 field_mT property +def test_DRV5056_field_mT(sensor: hall.DRV5056): + sensor.configure() + field = sensor.field_mT + assert field == sensor.field_strength + + +# Test DRV5056 data property +def test_DRV5056_data(sensor: hall.DRV5056): + with pytest.raises(NotImplementedError): + _ = sensor.data diff --git a/tests/test_sensors/test_sensors_base.py b/tests/test_sensors/test_sensors_base.py index d11d00e4..a37a9f95 100644 --- a/tests/test_sensors/test_sensors_base.py +++ b/tests/test_sensors/test_sensors_base.py @@ -3,7 +3,10 @@ import pytest from opensourceleg.sensors.base import ( + ADCBase, EncoderBase, + EncoderCounterBase, + HallBase, IMUBase, LoadcellBase, SensorBase, @@ -209,3 +212,86 @@ def mock_imu(): # Test IMUBase repr def test_imu_base_repr(mock_imu: MockIMU): assert mock_imu.__repr__() == "MockIMU[MockIMU]" + + +# Creating a Mock ADC Class +class MockADC(ADCBase, MockSensor): + def __init__(self, tag: str): + super().__init__(tag=tag) + + +@pytest.fixture +def mock_adc(): + return MockADC(tag="MockADC") + + +# Test ADCBase repr +def test_adc_base_repr(mock_adc: MockADC): + assert mock_adc.__repr__() == "ADCBase" + + +# Test ADCBase tag is still set correctly despite the overridden repr +def test_adc_base_tag(mock_adc: MockADC): + assert mock_adc.tag == "MockADC" + + +# Test ADCBase reset/calibrate are concrete no-ops (not abstract) +def test_adc_base_reset_and_calibrate_are_noops(mock_adc: MockADC): + assert mock_adc.reset() is None + assert mock_adc.calibrate() is None + + +# Test ADCBase offline config extends SensorBase +def test_adc_base_offline_methods(): + assert ADCBase._OFFLINE_METHODS == ["start", "stop", "update", "reset", "calibrate"] + # ADC doesn't add properties, so it inherits SensorBase's + assert ADCBase._OFFLINE_PROPERTIES == SensorBase._OFFLINE_PROPERTIES + assert ADCBase._OFFLINE_PROPERTY_DEFAULTS == {"data": None, "is_streaming": True} + + +# Test ADCBase context manager still works through the ADC subclass +def test_adc_base_context_manager(mock_adc: MockADC): + mock_adc.start = Mock() + mock_adc.stop = Mock() + with mock_adc as adc: + assert adc == mock_adc + mock_adc.start.assert_called_once() + mock_adc.stop.assert_not_called() + mock_adc.stop.assert_called_once() + + +# Creating a Mock EncoderCounter Class +class MockEncoderCounter(EncoderCounterBase, MockSensor): + def __init__(self, tag: str): + super().__init__(tag=tag) + + @property + def count(self): + pass + + +@pytest.fixture +def mock_encoder_counter(): + return MockEncoderCounter(tag="MockEncoderCounter") + + +# Test EncoderCounterBase repr +def test_encoder_counter_base_repr(mock_encoder_counter: MockEncoderCounter): + assert mock_encoder_counter.__repr__() == "EncoderCounterBase" + + +# Creating a Mock Hall Class +class MockHall(HallBase, MockSensor): + @property + def field_mT(self): + pass + + +@pytest.fixture +def mock_hall(): + return MockHall(tag="MockHall") + + +# Test HallBase repr +def test_hall_base_repr(mock_hall: MockHall): + assert mock_hall.__repr__() == "MockHall[MockHall]" From 275649997bb19d9024a87cc101e2f13d563fa546 Mon Sep 17 00:00:00 2001 From: Emily Date: Thu, 13 Aug 2026 14:56:13 -0400 Subject: [PATCH 04/17] doc: update README regard Hall Effect and Encoder Counter --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 679cbef6..a33f6603 100644 --- a/README.md +++ b/README.md @@ -58,14 +58,14 @@ The library currently supports the following hardware components: | AS5048B Encoder | ✅ | ✅ | ❌ | ✅ | | Lord Microstrain IMU | ✅ | ✅ | ❌ | ✅ | | SRI Loadcell | ✅ | ✅ | ❌ | ✅ | -| DRV5056 Hall Effect | ❌ | ❌ | ❌ | ❌ | -| LS7366R Encoder Counter | ❌ | ❌ | ❌ | ❌ | +| DRV5056 Hall Effect | ✅ | ❌ | ❌ | ❌ | +| LS7366R Encoder Counter | ✅ | ❌ | ❌ | ❌ | | Actuators | Unit Tests | Hardware Tests | Benchmarks | Documentation | | ------------- | ---------- | -------------- | ---------- | ------------- | -| Dephy Actpack | ⚠️ | ✅ | ⚠️ | ✅ | -| Moteus | ⚠️ | ⚠️ | ⚠️ | ✅ | -| TMotor | ❌ | ⚠️ | ❌ | ❌ | +| Dephy Actpack | ⚠️ | ✅ | ⚠️ | ✅ | +| Moteus | ⚠️ | ⚠️ | ⚠️ | ✅ | +| TMotor | ❌ | ⚠️ | ❌ | ❌ | > Legend: ✅ Complete/Available; ⚠️ Partial/In Progress; ❌ Not Yet Available; From 7b3d444c2e684c120af0193ec3456955d191390a Mon Sep 17 00:00:00 2001 From: KaHei Date: Thu, 13 Aug 2026 15:15:53 -0400 Subject: [PATCH 05/17] feat: add brushed motor actuator and update base actuator class --- opensourceleg/actuators/base.py | 48 +++ opensourceleg/actuators/brushed.py | 526 +++++++++++++++++++++++++++++ 2 files changed, 574 insertions(+) create mode 100644 opensourceleg/actuators/brushed.py diff --git a/opensourceleg/actuators/base.py b/opensourceleg/actuators/base.py index 2371939f..2ab7375d 100644 --- a/opensourceleg/actuators/base.py +++ b/opensourceleg/actuators/base.py @@ -1204,3 +1204,51 @@ def is_streaming(self) -> bool: True """ return self._is_streaming + + +class UnsupportedControlModeError(NotImplementedError): + """Raised when a control capability the actuator lacks is invoked.""" + + +class PositionControlActuatorBase(ActuatorBase): + """ActuatorBase specialized to position control only. + + Voltage, current, torque, and impedance methods are satisfied here once so + subclasses don't have to. They raise UnsupportedControlModeError if ever + called, and the corresponding telemetry properties do the same. Everything + else (position control, lifecycle, telemetry, mode machinery) is inherited + unchanged from ActuatorBase. + """ + + def set_motor_voltage(self, value: float) -> None: + raise UnsupportedControlModeError(f"{type(self).__name__} does not support voltage control.") + + def set_motor_current(self, value: float) -> None: + raise UnsupportedControlModeError(f"{type(self).__name__} does not support current control.") + + def set_motor_impedance(self, value: float) -> None: + raise UnsupportedControlModeError(f"{type(self).__name__} does not support impedance control.") + + def set_motor_torque(self, value: float) -> None: + raise UnsupportedControlModeError(f"{type(self).__name__} does not support torque control.") + + def set_output_torque(self, value: float) -> None: + raise UnsupportedControlModeError(f"{type(self).__name__} does not support torque control.") + + def set_current_gains(self, kp: float, ki: float, kd: float, ff: float) -> None: + raise UnsupportedControlModeError(f"{type(self).__name__} does not support current control.") + + def _set_impedance_gains(self, k: float, b: float) -> None: + raise UnsupportedControlModeError(f"{type(self).__name__} does not support impedance control.") + + @property + def motor_voltage(self) -> float: + raise UnsupportedControlModeError(f"{type(self).__name__} does not measure motor voltage.") + + @property + def motor_current(self) -> float: + raise UnsupportedControlModeError(f"{type(self).__name__} does not measure motor current.") + + @property + def motor_torque(self) -> float: + raise UnsupportedControlModeError(f"{type(self).__name__} does not measure motor torque.") diff --git a/opensourceleg/actuators/brushed.py b/opensourceleg/actuators/brushed.py new file mode 100644 index 00000000..6fa7208b --- /dev/null +++ b/opensourceleg/actuators/brushed.py @@ -0,0 +1,526 @@ +import time +from dataclasses import dataclass +from typing import Any, Callable, Optional + +import numpy as np +from gpiozero import OutputDevice, PWMOutputDevice +from gpiozero.pins.lgpio import LGPIOFactory + +from opensourceleg.actuators.base import ( + CONTROL_MODE_CONFIGS, + MOTOR_CONSTANTS, + ControlModeConfig, + PositionControlActuatorBase, +) +from opensourceleg.logging import LOGGER +from opensourceleg.sensors.base import EncoderCounterBase + +# Maxon x VNH7070AY specifications +MAXON_MODELS: dict[str, dict[str, Any]] = { + "B7E883374A15": { + "Curr_min": None, # A + "Curr_max": None, # A + "GEAR_RATIO": 6.6, + }, +} + + +@dataclass +class BrushedMotorState: + """Motor state data structure""" + + position: float = 0.0 # degrees + velocity: float = 0.0 # RPM + current: float = 0.0 # milliamps + temperature: float = 0.0 # celsius + error: int = 0 + + +# Simplified unit conversion functions +def degrees_to_radians(degrees: float) -> float: + """ + Convert degrees to radians + + Args: + degrees (float): Angle in degrees. + + Returns: + float: Angle in radians. + """ + return degrees * np.pi / 180.0 + + +def radians_to_degrees(radians: float) -> float: + """ + Convert radians to degrees + + Args: + radians (float): Angle in radians. + + Returns: + float: Angle in degrees. + """ + return radians * 180.0 / np.pi + + +def _maxon_position_mode_entry(maxon_actuator: "MaxonActuator") -> None: + LOGGER.debug(msg=f"[{maxon_actuator.tag}] Entering Position control mode.") + + +def _maxon_position_mode_exit(maxon_actuator: "MaxonActuator") -> None: + LOGGER.debug(msg=f"[{maxon_actuator.tag}] Exiting Position control mode.") + maxon_actuator.stop() + + +MAXON_CONTROL_MODE_CONFIGS = CONTROL_MODE_CONFIGS( + POSITION=ControlModeConfig( + entry_callback=_maxon_position_mode_entry, + exit_callback=_maxon_position_mode_exit, + has_gains=False, + max_gains=None, + ), + CURRENT=None, # CURRENT mode not supported + VELOCITY=None, # VELOCITY mode not supported + IDLE=None, # IDLE mode not supported + IMPEDANCE=None, # IMPEDANCE mode not supported + VOLTAGE=None, # VOLTAGE mode not supported +) + + +class MaxonActuator(PositionControlActuatorBase): + """ + Class for controlling a Maxon brushed motor with the VNH7070AY driver. + + Supports position control via PID and direct PWM commands. + Current, voltage, impedance, and velocity control modes are not supported. + """ + + def __init__( + self, + enable_pin: int = 12, + ina_pin: int = 24, + inb_pin: int = 25, + gear_ratio: float = 6.6, + frequency: int = 6000, + offline: bool = False, + pwm_maximum_command: float = 0.3, + pwm_minimum_command: float = 0.07, + pwm_lower_limit: float = 0.02, + tag: str = "maxon_actuator", + motor_constants: Optional[MOTOR_CONSTANTS] = None, + ) -> None: + """ + Initialize Maxon motor. + + Args: + enable_pin (int): GPIO pin number for the PWM enable signal. Defaults is 12. + ina_pin (int): GPIO pin number for motor direction input A. Defaults is 24. + inb_pin (int): GPIO pin number for motor direction input B. Defaults is 25. + gear_ratio (float): Gearbox reduction ratio. Defaults is 6.6. + frequency (int): PWM frequency in Hz. Defaults is 6000. + offline (bool): If True, skips GPIO initialization. Defaults is False. + pwm_maximum_command (float): Maximum allowable PWM duty cycle. Defaults is 0.3. + pwm_minimum_command (float): Minimum PWM duty cycle that produces motion. Defaults is 0.07. + pwm_lower_limit (float): PWM values below this threshold are set to zero. Defaults is 0.02. + tag (str): Human-readable identifier for this actuator instance. Defaults is "maxon_actuator". + motor_constants (optional): Motor constant parameters. Defaults is None. + """ + if motor_constants is None: + motor_constants = MOTOR_CONSTANTS( + MOTOR_COUNT_PER_REV=1024, + NM_PER_AMP=0.00652, + MAX_CASE_TEMPERATURE=85.0, + MAX_WINDING_TEMPERATURE=125.0, + ) + + super().__init__( + gear_ratio=gear_ratio, + offline=offline, + tag=tag, + motor_constants=motor_constants, + frequency=frequency, + ) + + self.enable_pin = enable_pin + self.ina_pin = ina_pin + self.inb_pin = inb_pin + + self.pwm_maximum_command = pwm_maximum_command + self.pwm_minimum_command = pwm_minimum_command + self.pwm_lower_limit = pwm_lower_limit + + if not self._is_offline: + self._factory = LGPIOFactory() + + self.speed_control = PWMOutputDevice(self.enable_pin, frequency=8000, initial_value=0) + self.inb = OutputDevice(self.inb_pin, initial_value=False) + self.ina = OutputDevice(self.ina_pin, initial_value=False) + LOGGER.info("Initialized Maxon x VNH7070AY.") + else: + LOGGER.info("Called brushed motor initialization in offline mode.") + + @property + def _CONTROL_MODE_CONFIGS(self) -> CONTROL_MODE_CONFIGS: + return MAXON_CONTROL_MODE_CONFIGS + + def start(self) -> None: + """ + Not supported. + """ + pass + + def stop(self) -> None: + """Stops the motor by setting PWM to zero and disabling direction outputs.""" + self.speed_control.value = 0 + self.ina.off() + self.inb.off() + + def update(self) -> None: + """Updates the actuator's data with encoder counter reading.""" + if self.encoder_counter: + self.motor_position_cts = self.encoder_counter.count + else: + self.motor_position_cts = 0.0 + self.motor_position_mm = self.cts_to_mm(self.motor_position_cts) + self.motor_position_perc = self.cts_to_perc(self.motor_position_cts) + + def set_motor_position(self, value: float = 0.0) -> None: + """Set the motor position. Not yet supported by this library.""" + raise NotImplementedError("Set motor position not implemented. Control the motor by setting PWM.") + + def set_output_impedance(self, value: float = 0.0) -> None: + """Set the output impedance. Not yet supported by this library.""" + raise NotImplementedError("Set output impedance not implemented. Control the motor by setting PWM.") + + def set_impedance_gains(self, k: float, b: float) -> None: + """Set impedance control gains. Not yet supported by this library.""" + raise NotImplementedError("Set impedance gains not implemented. Motor should be controlled by position or pwm.") + + def set_position_gains(self, k_p: float = 0.015, k_i: float = 2, k_d: float = 0.0001, ff: float = 0.0) -> None: + """Set position control gains.""" + self.k_p = k_p # Proportional gain + self.k_i = k_i # Integral gain + self.k_d = k_d # Derivative gain + + def _set_impedance_gains(self, k: float = 0.0, b: float = 0.0) -> None: + """Set impedance control gains. Not yet supported by this library.""" + raise NotImplementedError("Set impedance gains not implemented. Motor should be controlled by position or pwm.") + + def home( # type: ignore[override] + self, + homing_pwm: float = 0.25, + sample_rate: float = 0.05, + position_threshold: int = 200, + home_zero: bool = True, + timeout_s: float = 8.0, + callback: Optional[Callable[[], None]] = None, + ) -> None: + """ + Home the actuator by driving to a mechanical hard stop. + + Moves the motor at a fixed PWM until the encoder position stops + changing within the given threshold, then stops and optionally + executes a callback. The zero position corresponds to 0% stiffness + on the VSO; the hard stop corresponds to 100% stiffness. + + Args: + homing_pwm (float): PWM duty cycle applied during homing. Defaults to 0.25. + sample_rate (float): Time in seconds between encoder samples. Defaults to 0.05. + position_threshold (int): Maximum encoder count change between samples that is considered stationary. + Defaults to 200. + home_zero (bool): If True, home to the zero-stiffness position. If False, home + to the full-stiffness hard stop. Defaults to True. + timeout_s (float): Maximum homing duration in seconds. Defaults to 8.0. + callback (Optional[Callable[[], None]]): Optional callback function to be called when homing completes. + """ + time.sleep(1) + keep_going = True + + if home_zero: + LOGGER.info("Homing to zero position (0% stiffness).") + self.set_motor_direction_backward() + time.sleep(0.5) # Ensure direction is set before applying PWM + else: + LOGGER.info("Homing to hard stop (100% stiffness).") + self.set_motor_direction_forward() + time.sleep(0.5) # Ensure direction is set before applying PWM + + self.set_motor_pwm(homing_pwm) + + while keep_going: + self.update() + last_position = self.motor_position_cts + time.sleep(sample_rate) + + self.update() + error = self.motor_position_cts - last_position + if -position_threshold <= error <= position_threshold: + self.stop() + keep_going = False + + if callback is not None: + callback() + + @property + def motor_encoder_position_perc(self) -> float: + """ + Motor encoder position as a percentage of the full range of motion for the motor in one direction. + + Returns: + float: Position in percentage. + """ + self.update() + return self.motor_position_perc + + @property + def motor_position(self) -> float: + """ + Motor position in radians. + + Returns: + float : Motor position in radians. + """ + self.update() + return self.motor_position_cts * 2 * np.pi / 1024.0 + + @property + def motor_velocity(self) -> float: + """ + Motor velocity (radians / second). Not supported by this driver. + + Returns: + float: Always returns 0.0. + """ + LOGGER.warning("Motor velocity reading is not available.") + return 0.0 + + @property + def case_temperature(self) -> float: + """ + Motor case temperature in degrees Celsius. Not supported by this driver. + + The VNH7070AY has thermal shutdown protection, but it does not provide a real-time temperature + without an external sensor. + + Returns: + float: Always returns 0.0. + """ + LOGGER.warning("No temperature reading available for the motor casing.") + return 0.0 + + @property + def winding_temperature(self) -> float: + """ + Motor winding temperature in degrees Celsius. Not supported by this driver. + + The VNH7070AY has thermal shutdown protection but does not provide + real-time temperature without an external sensor. + + Returns: + float: Always returns 0.0. + """ + LOGGER.warning("No temperature reading available for the motor windings.") + return 0.0 + + def perc_to_cts(self, percentage: float) -> float: + """ + Convert a percentage of full range of motion to encoder counts. + + Args: + percentage (float): Position as a percentage. + + Returns: + float: Corresponding encoder count. + """ + return percentage * self.scale_perc + + def cts_to_perc(self, counts: float) -> float: + """ + Convert a percentage of full range of motion for the motor in one direction + to encoder counts. + + Args: + counts (float): Encoder count value. + + Returns: + float: Position as a percentage. + """ + return counts / self.scale_perc + + def mm_to_cts(self, mm: float) -> float: + """ + Convert linear displacement in millimeters to encoder counts. + + Args: + mm (float): Linear displacement in millimeters. + + Returns: + float: Corresponding encoder count. + """ + return mm * self.scale + + def cts_to_mm(self, counts: float) -> float: + """ + Convert a number of encoder counts to a number of mm moved assuming a + rotary to linear transmission like a lead screw. + + Args: + counts (float): Encoder count value. + + Returns: + float: Linear displacement in millimeters. + """ + return counts / self.scale + + def position_control_init(self, k_p: float = 0.015, k_i: float = 2, k_d: float = 0.0001) -> None: + """ + Initialize the PID position controller and reset all internal state. + + Args: + k_p (float): Proportional gain. Defaults to 0.015. + k_i (float): Integral gain. Defaults to 2. + k_d (float): Derivative gain. Defaults to 0.0001. + """ + self.set_position_gains(k_p, k_i, k_d) + self.error_encoder_last = 0.0 + self.d_term_last = 0.0 + self.d_term_filtered_last = 0.0 + self.i_term = 0.0 + self.last_pwm = 0.0 + + def position_control_config( + self, + scale_perc: float = 19972.65, + scale: float = 21281.976, + min_pos_error: float = 0.3, + slider_max_perc: float = 99.5, + slider_min_perc: float = 0.5, + allowable_coupler_drift: float = -0.2, + time_limit: float = 15.0, + ) -> None: + """ + Designed for the Variable Stiffness Orthosis + + After this time, the PWM will be set to zero, and the code assumes that the slider is jammed. + + Args: + scale_perc: encoder counts to 1% of full range of motion for the motor in one direction + scale: encoder conversion scale (counts to mm) + min_pos_error: Minimum desired change in slider position that will result in a motor command + slider_max_perc: [%] This is set slightly below 100% so that the spring support does not hit the hard stop. + slider_min_perc: [%] This is set slightly above 0% so that the spring support does not hit the coupler. + time_limit: [sec] maximum time for position control loop to execute (safety). + """ + LOGGER.info("Configuring position control.") + self.scale_perc = scale_perc + self.scale = scale + self.min_error = min_pos_error + self.slider_max_perc = slider_max_perc + self.slider_min_perc = slider_min_perc + self.allowable_coupler_drift = allowable_coupler_drift + self.time_limit = time_limit + + self.slider_min_counts = self.slider_min_perc * self.scale_perc + self.slider_max_counts = self.slider_max_perc * self.scale_perc + + self.slider_min_mm = self.slider_min_counts / self.scale + self.slider_max_mm = self.slider_max_counts / self.scale + + def lpfilter1(self, x: list[float], y_past: list[float]) -> float: + """ + Apply a first-order low-pass IIR filter to the derivative term. + + Used to low-pass filter the derivative term in the PID control of the VSO spring support. + + Args: + x (list[float]): Last two unfiltered derivative values [x_k, x_{k-1}]. + y_past (list[float]): Last one filtered derivative value [y_{k-1}]. + + Returns: + float: Filtered derivative value y. + """ + a1 = [1, -0.509525449494429] + b1 = [0.245237275252786, 0.245237275252786] + # send it last 1 filtered points and last 2 unfiltered points + y = -(a1[1] * y_past[0]) + b1[0] * x[0] + b1[1] * x[1] + return y + + def pid_ctrl_position(self, error_encoder: float, dt: float) -> float: + """ + Compute a PID PWM output given the current position error. + + Args: + error_encoder (float): Current position error in encoder counts. + dt (float): Time step in seconds since the last controller update. + + Returns: + float: PWM duty cycle command. + """ + p_term = self.k_p * error_encoder + + error_derivative = (error_encoder - self.error_encoder_last) / dt + self.error_encoder_last = error_encoder + d_term = error_derivative * self.k_d + d_term_filtered = self.lpfilter1([self.d_term_last, d_term], [self.d_term_filtered_last]) + self.d_term_last = d_term + self.d_term_filtered_last = d_term_filtered + + # only integrate when not saturated (prevent windup) + if -(self.pwm_maximum_command - 5) < self.last_pwm < (self.pwm_maximum_command - 5): + self.i_term = self.i_term + (self.k_i * error_encoder * dt) + + pwm_feedback = int(p_term + self.i_term + d_term_filtered) / 100 + self.last_pwm = pwm_feedback + + return pwm_feedback + + def check_coupler_drift(self) -> None: + """Check whether the lead screw coupler has drifted out of position.""" + if self.allowable_coupler_drift < self.motor_position_mm < 0: + LOGGER.warning("Coupler has drifted a little, but probably not an issue yet") + LOGGER.info(f"motor_position_mm: {self.motor_position_mm:.4f}") + + if self.motor_position_mm <= self.allowable_coupler_drift: + LOGGER.warning( + "Coupler has been pushed back towards the motor and should be reassembled. " + "Coupler should be flush with the lead screw, as far away from motor as " + "possible for accurate stiffness reports." + ) + LOGGER.info(f"motor_position_mm: {self.motor_position_mm:.4f}") + + def set_motor_direction_forward(self) -> None: + """Set the motor direction to be forwards.""" + self.ina.on() + self.inb.off() + + def set_motor_direction_backward(self) -> None: + """Set the motor direction to be backwards.""" + self.ina.off() + self.inb.on() + + def set_motor_pwm(self, pwm: float) -> None: + """Set the motor pwm rate.""" + if pwm > self.pwm_maximum_command: + LOGGER.info("PWM command above maximum. Setting to maximum.") + pwm = self.pwm_maximum_command + elif pwm < self.pwm_minimum_command and pwm >= self.pwm_lower_limit: + LOGGER.info("PWM command below minimum. Setting to minimum.") + pwm = self.pwm_minimum_command + elif pwm < self.pwm_lower_limit: + LOGGER.info("PWM command below lower limit. Setting to zero.") + pwm = 0.0 + + self.speed_control.value = pwm + + def set_motor_encoder(self, encoder_counter: EncoderCounterBase) -> None: + """ + Set the motor encoder counter. + + Args: + encoder_counter: Encoder counter instance providing its attribute. + """ + self.encoder_counter = encoder_counter + + +if __name__ == "__main__": + pass From cdacba29cf3aef4f27972e68e523fc8720ff17f4 Mon Sep 17 00:00:00 2001 From: KaHei Date: Thu, 13 Aug 2026 15:34:18 -0400 Subject: [PATCH 06/17] test: add unit test for Maxon Brushed actuator --- README.md | 1 + opensourceleg/actuators/brushed.py | 13 +- pyproject.toml | 1 + tests/test_actuators/test_actuators_base.py | 129 +++++++++ tests/test_actuators/test_brushed.py | 298 ++++++++++++++++++++ uv.lock | 16 ++ 6 files changed, 457 insertions(+), 1 deletion(-) create mode 100644 tests/test_actuators/test_brushed.py diff --git a/README.md b/README.md index a33f6603..7652b3f5 100644 --- a/README.md +++ b/README.md @@ -66,6 +66,7 @@ The library currently supports the following hardware components: | Dephy Actpack | ⚠️ | ✅ | ⚠️ | ✅ | | Moteus | ⚠️ | ⚠️ | ⚠️ | ✅ | | TMotor | ❌ | ⚠️ | ❌ | ❌ | +| Brushed Maxon | ✅ | ⚠️ | ❌ | ❌ | > Legend: ✅ Complete/Available; ⚠️ Partial/In Progress; ❌ Not Yet Available; diff --git a/opensourceleg/actuators/brushed.py b/opensourceleg/actuators/brushed.py index 6fa7208b..6b45d500 100644 --- a/opensourceleg/actuators/brushed.py +++ b/opensourceleg/actuators/brushed.py @@ -197,7 +197,18 @@ def set_impedance_gains(self, k: float, b: float) -> None: raise NotImplementedError("Set impedance gains not implemented. Motor should be controlled by position or pwm.") def set_position_gains(self, k_p: float = 0.015, k_i: float = 2, k_d: float = 0.0001, ff: float = 0.0) -> None: - """Set position control gains.""" + """ + Set PID gains for position control. + + Default values are tuned for the VSO configuration. Other actuator + or mechanism configurations will likely need retuning. + + Args: + k_p: Proportional gain. + k_i: Integral gain. + k_d: Derivative gain. + ff: Feedforward gain. + """ self.k_p = k_p # Proportional gain self.k_i = k_i # Integral gain self.k_d = k_d # Derivative gain diff --git a/pyproject.toml b/pyproject.toml index 84ccf48e..ef2c7d19 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -7,6 +7,7 @@ requires-python = ">=3.11,<4.0" readme = "README.md" dependencies = [ "gpiozero>=2.0.1.post3", + "lgpio>=0.2.2.0", "numpy>=1.24.3,<2", "pandas>=2.2.3,<3", ] diff --git a/tests/test_actuators/test_actuators_base.py b/tests/test_actuators/test_actuators_base.py index 879d85fb..c6556576 100644 --- a/tests/test_actuators/test_actuators_base.py +++ b/tests/test_actuators/test_actuators_base.py @@ -15,7 +15,9 @@ ControlGains, ControlModeConfig, MethodWithRequiredModes, + PositionControlActuatorBase, T, + UnsupportedControlModeError, requires, ) from opensourceleg.logging.exceptions import ControlModeException @@ -699,3 +701,130 @@ def test_offline_mode_with_context_manager(mock_actuator_offline: MockActuator): actuator.update() pos = actuator.motor_position # Should return 0.0 assert pos == 0.0 + + +# Test PositionControlActuatorBase +class MockPositionActuator(PositionControlActuatorBase): + """Fills in only the still-abstract members so the position-only base can be built.""" + + @property + def _CONTROL_MODE_CONFIGS(self): + return CONTROL_MODE_CONFIGS() # all-None configs; mode switching still works + + def start(self): + pass + + def stop(self): + pass + + def update(self): + pass + + def set_motor_position(self, value): + pass + + def set_position_gains(self, kp, ki, kd, ff): + pass + + def home(self): + pass + + @property + def motor_position(self): + return 0.0 + + @property + def motor_velocity(self): + return 0.0 + + @property + def case_temperature(self): + return 0.0 + + @property + def winding_temperature(self): + return 0.0 + + +@pytest.fixture +def mock_position_actuator(): + # Online (offline=False) so the UnsupportedControlModeError bodies actually run; + # offline mode would stub the hardware methods and mask them. + return MockPositionActuator( + "test_position_actuator", + 10.0, + MOTOR_CONSTANTS( + MOTOR_COUNT_PER_REV=1000, + NM_PER_AMP=0.1, + MAX_CASE_TEMPERATURE=100.0, + MAX_WINDING_TEMPERATURE=150.0, + ), + ) + + +def test_unsupported_control_mode_error_is_notimplemented(): + assert issubclass(UnsupportedControlModeError, NotImplementedError) + + +def test_position_only_actuator_init(mock_position_actuator: MockPositionActuator): + assert mock_position_actuator.tag == "test_position_actuator" + assert mock_position_actuator.gear_ratio == 10.0 + assert mock_position_actuator.mode == CONTROL_MODES.IDLE + + +# The unsupported *properties* aren't mode-gated, so they raise in any mode. +def test_position_only_unsupported_properties(mock_position_actuator: MockPositionActuator): + with pytest.raises(UnsupportedControlModeError): + _ = mock_position_actuator.motor_voltage + with pytest.raises(UnsupportedControlModeError): + _ = mock_position_actuator.motor_current + with pytest.raises(UnsupportedControlModeError): + _ = mock_position_actuator.motor_torque + + +# The unsupported *methods* are mode-gated by ActuatorBase, so we must first enter a +# mode where they're allowed — otherwise we'd get ControlModeException, not the +# UnsupportedControlModeError we're actually testing for. +def test_position_only_unsupported_voltage(mock_position_actuator: MockPositionActuator): + mock_position_actuator.set_control_mode(CONTROL_MODES.VOLTAGE) + with pytest.raises(UnsupportedControlModeError): + mock_position_actuator.set_motor_voltage(1.0) + + +def test_position_only_unsupported_current(mock_position_actuator: MockPositionActuator): + mock_position_actuator.set_control_mode(CONTROL_MODES.CURRENT) + with pytest.raises(UnsupportedControlModeError): + mock_position_actuator.set_motor_current(1.0) + + +def test_position_only_unsupported_torque(mock_position_actuator: MockPositionActuator): + mock_position_actuator.set_control_mode(CONTROL_MODES.TORQUE) + with pytest.raises(UnsupportedControlModeError): + mock_position_actuator.set_motor_torque(1.0) + with pytest.raises(UnsupportedControlModeError): + mock_position_actuator.set_output_torque(1.0) + + +def test_position_only_unsupported_current_gains(mock_position_actuator: MockPositionActuator): + mock_position_actuator.set_control_mode(CONTROL_MODES.CURRENT) + with pytest.raises(UnsupportedControlModeError): + mock_position_actuator.set_current_gains(1.0, 0.1, 0.01, 0.0) + + +def test_position_only_unsupported_impedance(mock_position_actuator: MockPositionActuator): + mock_position_actuator.set_control_mode(CONTROL_MODES.IMPEDANCE) + with pytest.raises(UnsupportedControlModeError): + mock_position_actuator.set_motor_impedance(1.0) + + +def test_position_only_unsupported_impedance_gains(mock_position_actuator: MockPositionActuator): + # _set_impedance_gains is not in _METHOD_REQUIRED_MODES, so it's never gated. + with pytest.raises(UnsupportedControlModeError): + mock_position_actuator._set_impedance_gains(1.0, 1.0) + + +def test_position_only_supported_position(mock_position_actuator: MockPositionActuator): + mock_position_actuator.set_control_mode(CONTROL_MODES.POSITION) + mock_position_actuator.set_motor_position(1.0) # should NOT raise + assert mock_position_actuator.motor_position == 0.0 + assert mock_position_actuator.motor_velocity == 0.0 diff --git a/tests/test_actuators/test_brushed.py b/tests/test_actuators/test_brushed.py new file mode 100644 index 00000000..5a8f18ff --- /dev/null +++ b/tests/test_actuators/test_brushed.py @@ -0,0 +1,298 @@ +from unittest.mock import MagicMock, patch + +import numpy as np +import pytest + +from opensourceleg.actuators.base import ( + CONTROL_MODES, + MOTOR_CONSTANTS, + UnsupportedControlModeError, +) +from opensourceleg.actuators.brushed import ( + MaxonActuator, + degrees_to_radians, + radians_to_degrees, +) + +# Valid constants (winding > case) so construction passes MOTOR_CONSTANTS validation. +VALID_MOTOR_CONSTANTS = MOTOR_CONSTANTS( + MOTOR_COUNT_PER_REV=1024, + NM_PER_AMP=0.00652, + MAX_CASE_TEMPERATURE=100.0, + MAX_WINDING_TEMPERATURE=125.0, +) + + +# actuator fixture +@pytest.fixture +def mock_gpio(): + """ + Patch the gpiozero devices so MaxonActuator can be built online without hardware. + OutputDevice is called for inb first, then ina (per __init__ order), so side_effect + hands back distinct mocks and the two direction pins stay distinguishable. + """ + with ( + patch("opensourceleg.actuators.brushed.LGPIOFactory"), + patch("opensourceleg.actuators.brushed.PWMOutputDevice") as mock_pwm, + patch("opensourceleg.actuators.brushed.OutputDevice") as mock_out, + ): + speed_control = MagicMock(name="speed_control") + inb = MagicMock(name="inb") + ina = MagicMock(name="ina") + mock_pwm.return_value = speed_control + mock_out.side_effect = [inb, ina] + yield {"speed_control": speed_control, "ina": ina, "inb": inb} + + +@pytest.fixture +def maxon(mock_gpio): + """A default online MaxonActuator backed by mocked gpiozero devices.""" + return MaxonActuator(motor_constants=VALID_MOTOR_CONSTANTS) + + +# Module-level unit conversion helpers +def test_degrees_to_radians(): + assert degrees_to_radians(180.0) == pytest.approx(np.pi) + assert degrees_to_radians(0.0) == 0.0 + + +def test_radians_to_degrees(): + assert radians_to_degrees(np.pi) == pytest.approx(180.0) + assert radians_to_degrees(0.0) == 0.0 + + +def test_degrees_radians_roundtrip(): + assert radians_to_degrees(degrees_to_radians(45.0)) == pytest.approx(45.0) + + +# Construction / configuration +def test_default_constants_no_raise(): + actuator = MaxonActuator() + assert actuator is not None + + +def test_maxon_init(maxon: MaxonActuator): + assert maxon.tag == "maxon_actuator" + assert maxon.gear_ratio == 6.6 + assert maxon.frequency == 6000 + assert maxon.enable_pin == 12 + assert maxon.ina_pin == 24 + assert maxon.inb_pin == 25 + assert maxon.pwm_maximum_command == 0.3 + assert maxon.pwm_minimum_command == 0.07 + assert maxon.pwm_lower_limit == 0.02 + + +def test_maxon_control_mode_configs(maxon: MaxonActuator): + configs = maxon._CONTROL_MODE_CONFIGS + assert configs.POSITION is not None + assert configs.CURRENT is None + assert configs.VOLTAGE is None + assert configs.IMPEDANCE is None + assert configs.VELOCITY is None + + +# Unsupported control modes(should raise error) +def test_maxon_voltage_unsupported(maxon: MaxonActuator): + maxon.set_control_mode(CONTROL_MODES.VOLTAGE) + with pytest.raises(UnsupportedControlModeError): + maxon.set_motor_voltage(1.0) + + +def test_maxon_current_unsupported(maxon: MaxonActuator): + maxon.set_control_mode(CONTROL_MODES.CURRENT) + with pytest.raises(UnsupportedControlModeError): + maxon.set_motor_current(1.0) + + +def test_maxon_set_motor_position_not_implemented(maxon: MaxonActuator): + maxon.set_control_mode(CONTROL_MODES.POSITION) + with pytest.raises(NotImplementedError): + maxon.set_motor_position(1.0) + + +def test_maxon_set_output_impedance_not_implemented(maxon: MaxonActuator): + maxon.set_control_mode(CONTROL_MODES.IMPEDANCE) + with pytest.raises(NotImplementedError): + maxon.set_output_impedance(1.0) + + +def test_maxon_impedance_gains_not_implemented(maxon: MaxonActuator): + # _set_impedance_gains is not mode-gated. + with pytest.raises(NotImplementedError): + maxon._set_impedance_gains(1.0, 1.0) + # set_impedance_gains IS mode-gated (IMPEDANCE); enter the mode first. + maxon.set_control_mode(CONTROL_MODES.IMPEDANCE) + with pytest.raises(NotImplementedError): + maxon.set_impedance_gains(1.0, 1.0) + + +# Gains / PWM / direction +def test_maxon_set_position_gains(maxon: MaxonActuator): + maxon.set_control_mode(CONTROL_MODES.POSITION) + maxon.set_position_gains(k_p=0.02, k_i=1.0, k_d=0.001) + assert maxon.k_p == 0.02 + assert maxon.k_i == 1.0 + assert maxon.k_d == 0.001 + + +def test_maxon_pwm_within_range(maxon: MaxonActuator): + maxon.set_motor_pwm(0.15) + assert maxon.speed_control.value == 0.15 + + +def test_maxon_pwm_clamps_to_max(maxon: MaxonActuator): + maxon.set_motor_pwm(0.5) # above the 0.3 maximum + assert maxon.speed_control.value == 0.3 + + +def test_maxon_pwm_bumped_to_min(maxon: MaxonActuator): + maxon.set_motor_pwm(0.05) # in [lower_limit, minimum) -> minimum + assert maxon.speed_control.value == 0.07 + + +def test_maxon_pwm_below_lower_limit_zeroed(maxon: MaxonActuator): + maxon.set_motor_pwm(0.01) # below the 0.02 lower limit -> 0 + assert maxon.speed_control.value == 0.0 + + +def test_maxon_direction_forward(maxon: MaxonActuator): + maxon.set_motor_direction_forward() + maxon.ina.on.assert_called_once() + maxon.inb.off.assert_called_once() + + +def test_maxon_direction_backward(maxon: MaxonActuator): + maxon.set_motor_direction_backward() + maxon.ina.off.assert_called_once() + maxon.inb.on.assert_called_once() + + +def test_maxon_stop(maxon: MaxonActuator): + maxon.stop() + assert maxon.speed_control.value == 0 + maxon.ina.off.assert_called_once() + maxon.inb.off.assert_called_once() + + +# Encoder / update / conversions +def test_maxon_update_with_encoder(maxon: MaxonActuator): + encoder = MagicMock() + encoder.count = 5000 + maxon.set_motor_encoder(encoder) + maxon.position_control_config() # sets scale / scale_perc used by cts_to_* helpers + maxon.update() + assert maxon.motor_position_cts == 5000 + assert maxon.motor_position_mm == maxon.cts_to_mm(5000) + assert maxon.motor_position_perc == maxon.cts_to_perc(5000) + + +def test_maxon_unit_conversions(maxon: MaxonActuator): + maxon.position_control_config(scale_perc=100.0, scale=200.0) + assert maxon.perc_to_cts(2.0) == 200.0 + assert maxon.cts_to_perc(200.0) == 2.0 + assert maxon.mm_to_cts(3.0) == 600.0 + assert maxon.cts_to_mm(600.0) == 3.0 + + +def test_maxon_position_control_config_derived(maxon: MaxonActuator): + maxon.position_control_config( + scale_perc=100.0, + scale=200.0, + slider_min_perc=0.5, + slider_max_perc=99.5, + ) + assert maxon.slider_min_counts == 0.5 * 100.0 + assert maxon.slider_max_counts == 99.5 * 100.0 + assert maxon.slider_min_mm == (0.5 * 100.0) / 200.0 + assert maxon.slider_max_mm == (99.5 * 100.0) / 200.0 + + +def test_maxon_motor_position_property(maxon: MaxonActuator): + encoder = MagicMock() + encoder.count = 1024 + maxon.set_motor_encoder(encoder) + maxon.position_control_config() + # motor_position = counts * 2*pi / 1024, so 1024 counts -> 2*pi rad. + assert maxon.motor_position == pytest.approx(2 * np.pi) + + +def test_maxon_unsupported_readings(maxon: MaxonActuator): + assert maxon.motor_velocity == 0.0 + assert maxon.case_temperature == 0.0 + assert maxon.winding_temperature == 0.0 + + +# PID controller +def test_maxon_position_control_init_resets_state(maxon: MaxonActuator): + # position_control_init calls set_position_gains internally, which is POSITION-gated. + maxon.set_control_mode(CONTROL_MODES.POSITION) + maxon.position_control_init(k_p=0.02, k_i=1.0, k_d=0.001) + assert maxon.k_p == 0.02 + assert maxon.error_encoder_last == 0.0 + assert maxon.d_term_last == 0.0 + assert maxon.d_term_filtered_last == 0.0 + assert maxon.i_term == 0.0 + assert maxon.last_pwm == 0.0 + + +def test_maxon_pid_returns_float_and_updates_last_pwm(maxon: MaxonActuator): + maxon.set_control_mode(CONTROL_MODES.POSITION) + maxon.position_control_init() + pwm = maxon.pid_ctrl_position(error_encoder=1000.0, dt=0.01) + assert isinstance(pwm, float) + assert maxon.last_pwm == pwm + + +def test_maxon_pid_i_term_never_integrates(maxon: MaxonActuator): + # Documents the anti-windup guard bug: with pwm_maximum_command == 0.3 the window + # (0.3 - 5) is negative, so the integrator condition can never be true. + maxon.set_control_mode(CONTROL_MODES.POSITION) + maxon.position_control_init() + maxon.pid_ctrl_position(error_encoder=1000.0, dt=0.01) + assert maxon.i_term == 0.0 + + +# Homing (loop + hardware, with time.sleep patched out) +def test_maxon_home_stops_when_stable(maxon: MaxonActuator): + encoder = MagicMock() + encoder.count = 100 # constant -> position immediately "stable" -> one loop pass + maxon.set_motor_encoder(encoder) + maxon.position_control_config() + with patch("opensourceleg.actuators.brushed.time.sleep"): + maxon.home() + # home() ends by calling stop(), which zeros PWM. + assert maxon.speed_control.value == 0 + + +def test_maxon_home_invokes_callback(maxon: MaxonActuator): + encoder = MagicMock() + encoder.count = 0 + maxon.set_motor_encoder(encoder) + maxon.position_control_config() + callback = MagicMock() + with patch("opensourceleg.actuators.brushed.time.sleep"): + maxon.home(callback=callback) + callback.assert_called_once() + + +# Offline integration (mirrors the DephyActuator offline example) +@pytest.fixture +def maxon_offline(): + # No gpiozero mocking needed: offline mode skips the hardware branch entirely. + return MaxonActuator(offline=True, motor_constants=VALID_MOTOR_CONSTANTS) + + +def test_maxon_offline_flags(maxon_offline: MaxonActuator): + assert maxon_offline.is_offline is True + assert maxon_offline.is_open is True + assert maxon_offline.is_streaming is True + assert maxon_offline.tag == "maxon_actuator" + + +def test_maxon_offline_hardware_methods_are_noops(maxon_offline: MaxonActuator): + # OfflineMixin stubs these, so they run without any gpiozero devices present. + maxon_offline.start() + maxon_offline.stop() + maxon_offline.update() + maxon_offline.home() diff --git a/uv.lock b/uv.lock index e393c6d5..2417323d 100644 --- a/uv.lock +++ b/uv.lock @@ -640,6 +640,20 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/31/b4/b9b800c45527aadd64d5b442f9b932b00648617eb5d63d2c7a6587b7cafc/jmespath-1.0.1-py3-none-any.whl", hash = "sha256:02e2e4cc71b5bcab88332eebf907519190dd9e6e82107fa7f83b1003a6252980", size = 20256, upload-time = "2022-06-17T18:00:10.251Z" }, ] +[[package]] +name = "lgpio" +version = "0.2.2.0" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/56/33/26ec2e8049eaa2f077bf23a12dc61ca559fbfa7bea0516bf263d657ae275/lgpio-0.2.2.0.tar.gz", hash = "sha256:11372e653b200f76a0b3ef8a23a0735c85ec678a9f8550b9893151ed0f863fff", size = 90087, upload-time = "2024-03-29T21:59:55.901Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/78/4e/5721ae44b29e4fe9175f68c881694e3713066590739a7c87f8cee2835c25/lgpio-0.2.2.0-cp311-cp311-linux_armv7l.whl", hash = "sha256:5b3c403e1fba9c17d178f1bde102726c548fc5c4fc1ccf5ec3e18f3c08e07e04", size = 382992, upload-time = "2024-03-29T22:00:45.039Z" }, + { url = "https://files.pythonhosted.org/packages/88/53/e57a22fe815fc68d0991655c1105b8ed872a68491d32e4e0e7d10ffb5c4d/lgpio-0.2.2.0-cp311-cp311-manylinux_2_34_aarch64.whl", hash = "sha256:a2f71fb95b149d8ac82c7c6bae70f054f6dc42a006ad35c90c7d8e54921fbcf4", size = 364848, upload-time = "2024-04-01T22:49:45.889Z" }, + { url = "https://files.pythonhosted.org/packages/a4/71/11f4e3d76400e4ca43f9f9b014f5a86d9a265340c0bea45cce037277eb34/lgpio-0.2.2.0-cp311-cp311-manylinux_2_34_x86_64.whl", hash = "sha256:e9f4f3915abe5ae0ffdb4b96f485076d80a663876d839e2d3fd9218a71b9873e", size = 370183, upload-time = "2024-04-13T14:08:14.139Z" }, + { url = "https://files.pythonhosted.org/packages/fe/73/e56c9afb845df53492d42bdea01df9895272bccfdd5128f34719c3a07990/lgpio-0.2.2.0-cp312-cp312-linux_armv7l.whl", hash = "sha256:6c65ac42e878764d04a71ed12fe6d46089b36e9e8127722bf29bb2e4bc91de22", size = 383956, upload-time = "2024-03-29T22:00:47.315Z" }, + { url = "https://files.pythonhosted.org/packages/3b/1c/becd00f66d2c65feed9a668ff9d91732394cb6baba7bec505d55de0e30c9/lgpio-0.2.2.0-cp312-cp312-manylinux_2_34_aarch64.whl", hash = "sha256:d907db79292c721c605af08187385ddb3b7af09907e1ffca56cf0cd6558ace0a", size = 366058, upload-time = "2024-04-01T22:49:47.615Z" }, + { url = "https://files.pythonhosted.org/packages/4c/7a/e3b4e5225c9792c4092b2cc07504746acbe62d0a8e4cb023bdf65f6430cf/lgpio-0.2.2.0-cp312-cp312-manylinux_2_34_x86_64.whl", hash = "sha256:2aadff092f642fcdada8457c158f87259dfda3a89ec19bae0b99ff22b34aac4b", size = 372103, upload-time = "2024-04-13T14:08:16.351Z" }, +] + [[package]] name = "markdown" version = "3.8.2" @@ -942,6 +956,7 @@ version = "3.5.0" source = { editable = "." } dependencies = [ { name = "gpiozero" }, + { name = "lgpio" }, { name = "numpy" }, { name = "pandas" }, ] @@ -1008,6 +1023,7 @@ requires-dist = [ { name = "gpiozero", specifier = ">=2.0.1.post3" }, { name = "grpcio", marker = "extra == 'messaging'", specifier = ">=1.65.5,<2" }, { name = "grpcio-tools", marker = "extra == 'messaging'", specifier = ">=1.65.5,<2" }, + { name = "lgpio", specifier = ">=0.2.2.0" }, { name = "moteus", marker = "extra == 'moteus'", specifier = ">=0.3.72,<0.4" }, { name = "moteus-pi3hat", marker = "platform_machine == 'aarch64' and sys_platform == 'linux' and extra == 'moteus'", specifier = ">=0.3.29,<0.4" }, { name = "numpy", specifier = ">=1.24.3,<2" }, From 2085087f2a299611514320543d877700e1ef99dd Mon Sep 17 00:00:00 2001 From: KaHei Date: Thu, 13 Aug 2026 15:48:42 -0400 Subject: [PATCH 07/17] test: add unit test for Maxon Brushed actuator --- opensourceleg/actuators/brushed.py | 2 +- tests/test_actuators/test_brushed.py | 24 +++++++++++++++++++++--- 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/opensourceleg/actuators/brushed.py b/opensourceleg/actuators/brushed.py index 6b45d500..dc263417 100644 --- a/opensourceleg/actuators/brushed.py +++ b/opensourceleg/actuators/brushed.py @@ -477,7 +477,7 @@ def pid_ctrl_position(self, error_encoder: float, dt: float) -> float: self.d_term_filtered_last = d_term_filtered # only integrate when not saturated (prevent windup) - if -(self.pwm_maximum_command - 5) < self.last_pwm < (self.pwm_maximum_command - 5): + if -(self.pwm_maximum_command - 0.05) < self.last_pwm < (self.pwm_maximum_command - 0.05): self.i_term = self.i_term + (self.k_i * error_encoder * dt) pwm_feedback = int(p_term + self.i_term + d_term_filtered) / 100 diff --git a/tests/test_actuators/test_brushed.py b/tests/test_actuators/test_brushed.py index 5a8f18ff..1c7e40ce 100644 --- a/tests/test_actuators/test_brushed.py +++ b/tests/test_actuators/test_brushed.py @@ -244,12 +244,30 @@ def test_maxon_pid_returns_float_and_updates_last_pwm(maxon: MaxonActuator): assert maxon.last_pwm == pwm -def test_maxon_pid_i_term_never_integrates(maxon: MaxonActuator): - # Documents the anti-windup guard bug: with pwm_maximum_command == 0.3 the window - # (0.3 - 5) is negative, so the integrator condition can never be true. +def test_maxon_pid_i_term_integrates_when_not_saturated(maxon: MaxonActuator): + # With the corrected anti-windup guard (0.05 margin instead of 5), + # the integrator now updates when last_pwm sits inside the unsaturated window. maxon.set_control_mode(CONTROL_MODES.POSITION) maxon.position_control_init() + + # Sanity check on the precondition: last_pwm starts at 0.0, which should + # fall inside the window for any reasonable pwm_maximum_command. + assert -(maxon.pwm_maximum_command - 0.05) < maxon.last_pwm < (maxon.pwm_maximum_command - 0.05) + + maxon.pid_ctrl_position(error_encoder=1000.0, dt=0.01) + + expected_i_term = maxon.k_i * 1000.0 * 0.01 + assert maxon.i_term == pytest.approx(expected_i_term) + + +def test_maxon_pid_i_term_holds_when_saturated(maxon: MaxonActuator): + # When last_pwm is outside the guard window, the integrator must not update. + maxon.set_control_mode(CONTROL_MODES.POSITION) + maxon.position_control_init() + + maxon.last_pwm = maxon.pwm_maximum_command # force saturation maxon.pid_ctrl_position(error_encoder=1000.0, dt=0.01) + assert maxon.i_term == 0.0 From f3e54dbb77d5b4080600f9be6fdd41a5ebbe4ecf Mon Sep 17 00:00:00 2001 From: KaHei Date: Thu, 13 Aug 2026 15:55:32 -0400 Subject: [PATCH 08/17] docs: add tutorial sample code for brushed motor actuator --- .../actuators/brushed/commanding_position.py | 135 ++++++++++++++++++ tutorials/actuators/brushed/homing_motor.py | 40 ++++++ 2 files changed, 175 insertions(+) create mode 100644 tutorials/actuators/brushed/commanding_position.py create mode 100644 tutorials/actuators/brushed/homing_motor.py diff --git a/tutorials/actuators/brushed/commanding_position.py b/tutorials/actuators/brushed/commanding_position.py new file mode 100644 index 00000000..49582d06 --- /dev/null +++ b/tutorials/actuators/brushed/commanding_position.py @@ -0,0 +1,135 @@ +import time + +import numpy as np + +from opensourceleg.actuators.base import CONTROL_MODES +from opensourceleg.actuators.brushed import MaxonActuator +from opensourceleg.logging.logger import Logger +from opensourceleg.sensors.encoderCounter import LS7366R + +TIME_TO_STEP = 1.0 +FREQUENCY = 200 +DT = 1 / FREQUENCY + +TARGET_PERC = [30.0] # Target stiffness position in percentage. Edit here to change target position. + + +def _clamp_target_cts(actuator: MaxonActuator, desired_cts: int) -> int: + """Clamp a target position (counts) into the safe slider range.""" + desired_mm = actuator.cts_to_mm(desired_cts) + if desired_mm > actuator.slider_max_mm: + return actuator.slider_max_counts + if desired_mm < actuator.slider_min_mm: + return actuator.slider_min_counts + return desired_cts + + +def _command_pwm(actuator: MaxonActuator, pwm: float) -> None: + """Set motor direction from the sign of `pwm`, then apply its magnitude.""" + if pwm < 0.0: + actuator.set_motor_direction_backward() # toward lower % + else: + actuator.set_motor_direction_forward() # toward higher % + + actuator.set_motor_pwm(np.abs(pwm)) + + +def go_to_position(actuator: MaxonActuator, target_perc: float, position_logger: Logger) -> bool: + """Command the motor to move the slider to `target_perc`. + + Returns True if it arrived within min_error before the time limit, False if it timed out. + + Direction convention assumes the encoder counts UP in the forward direction. + """ + desired_cts = _clamp_target_cts(actuator, actuator.perc_to_cts(target_perc)) + desired_mm = actuator.cts_to_mm(desired_cts) + + start_time = time.time() + last_time = start_time + + while True: + try: + actuator.update() + actuator.check_coupler_drift() + + # TODO: replace with a real check_ankle_position() on the orthosis. + ankle_in_range = True + if not ankle_in_range: + actuator.stop() + position_logger.warning("Ankle out of range - aborting slider motion.") + return False + + if np.abs(desired_mm - actuator.motor_position_mm) < actuator.min_error: + actuator.stop() + return True + + current_time = time.time() + dt = current_time - last_time + if dt <= 0.0: + dt = 1e-6 # guard against divide-by-zero in the PID + last_time = current_time + + if current_time - start_time > actuator.time_limit: + position_logger.warning("Slider may be jammed - check prototype (PWM set to zero for safety).") + actuator.stop() + return False + + error_cts = int(desired_cts - actuator.motor_position_cts) + pwm = actuator.pid_ctrl_position(error_cts, dt) + + if pwm == 0.0: + actuator.stop() + return True + + _command_pwm(actuator, pwm) + time.sleep(DT) + + except KeyboardInterrupt: + actuator.stop() + position_logger.warning("KeyboardInterrupt during slider motion.") + return False + + +def position_control(): + position_logger = Logger( + log_path="./logs", + file_name="position_control", + ) + + # initialize actuator and encodercounter + actuator = MaxonActuator( + frequency=FREQUENCY, + offline=False, + ) + + encoder_counter = LS7366R() + + with actuator: + actuator.set_motor_encoder(encoder_counter) + + actuator.position_control_config() + actuator.set_control_mode(CONTROL_MODES.POSITION) + actuator.position_control_init() + + if not actuator._is_homed: + position_logger.info("Actuator homing to 0% stiffness.") + actuator.home(home_zero=True) + encoder_counter.clear_counter() + actuator._is_homed = True + + actuator.update() + actuator.check_coupler_drift() + position_logger.info(f"Homing complete. position = {actuator.motor_position_mm:.3f} mm)") + + for target_perc in TARGET_PERC: + arrived = go_to_position(actuator, target_perc, position_logger) + actuator.update() + position_logger.info( + f"target {target_perc:.2f} % -> {'arrived' if arrived else 'TIMED OUT'} " + f"at {actuator.motor_position_mm:.3f} mm" + ) + time.sleep(1.0) + + +if __name__ == "__main__": + position_control() diff --git a/tutorials/actuators/brushed/homing_motor.py b/tutorials/actuators/brushed/homing_motor.py new file mode 100644 index 00000000..b4c9a2a7 --- /dev/null +++ b/tutorials/actuators/brushed/homing_motor.py @@ -0,0 +1,40 @@ +from opensourceleg.actuators.brushed import MaxonActuator +from opensourceleg.logging.logger import Logger +from opensourceleg.sensors.encoderCounter import LS7366R + +TIME_TO_STEP = 1.0 +FREQUENCY = 200 +DT = 1 / FREQUENCY + + +def home_motor(): + homing_logger = Logger(log_path="./logs", file_name="home_motor") + + # initialize actuator and encodercounter + actuator = MaxonActuator(frequency=FREQUENCY, offline=False) + encoder_counter = LS7366R() + + # set up motor and start homing + with actuator: + actuator.set_motor_encoder(encoder_counter) + actuator.position_control_config() + + if actuator._is_homed: + homing_logger.info("Actuator already homed to 0% stiffness.") + else: + homing_logger.info("Actuator homing to 0% stiffness.") + actuator.home(home_zero=True) + encoder_counter.clear_counter() + actuator._is_homed = True + + actuator.update() + actuator.check_coupler_drift() + homing_logger.info( + f"Homing complete. position = " + f"{actuator.cts_to_perc(actuator.motor_position_cts):.2f}% " + f"({actuator.motor_position_mm:.3f} mm)" + ) + + +if __name__ == "__main__": + home_motor() From 9d12f01f1c7b6f5a506a7003f222a6ee66a2f014 Mon Sep 17 00:00:00 2001 From: Emily Date: Fri, 14 Aug 2026 13:45:45 -0400 Subject: [PATCH 09/17] test: add unit tests to improve codecov coverage --- tests/test_sensors/test_adc.py | 153 +++++++++++++++++++++++++++++++++ 1 file changed, 153 insertions(+) diff --git a/tests/test_sensors/test_adc.py b/tests/test_sensors/test_adc.py index af1fc293..4fb3b1b6 100644 --- a/tests/test_sensors/test_adc.py +++ b/tests/test_sensors/test_adc.py @@ -449,6 +449,85 @@ def test_read_converted_data_crc_error_raises(adc, mock_spi): adc.read_converted_data() +def test_read_converted_data_crc_only_success(adc, mock_spi): + # byte_options == 1: CRC enabled, status disabled + adc._register_map[ADS114S0x._REG_ADDR_SYS] = ADS114S0x._ADS_CRC_MASK + mock_spi.xfer2.return_value = [0x12, 0x34, 0x00, 0xAB] # msb, mid, lsb, crc + + with patch.object(adc, "get_crc", return_value=0) as get_crc: + code16, status = adc.read_converted_data() + + assert code16 == 0x1234 + assert status is None + get_crc.assert_called_once_with([0x12, 0x34, 0x00, 0xAB], 4) + mock_spi.xfer2.assert_called_once_with([0, 0, 0, 0]) + + +def test_read_converted_data_status_and_crc_success(adc, mock_spi): + # byte_options == 3: both status and CRC enabled + adc._register_map[ADS114S0x._REG_ADDR_SYS] = ADS114S0x._ADS_SENDSTATUS_MASK | ADS114S0x._ADS_CRC_MASK + mock_spi.xfer2.return_value = [0x80, 0x12, 0x34, 0x00, 0xAB] # status, msb, mid, lsb, crc + + with patch.object(adc, "get_crc", return_value=0) as get_crc: + code16, status = adc.read_converted_data() + + assert status == 0x80 + assert code16 == 0x1234 + get_crc.assert_called_once_with([0x80, 0x12, 0x34, 0x00, 0xAB], 5) + + +def test_read_converted_data_status_and_crc_error_raises(adc, mock_spi): + adc._register_map[ADS114S0x._REG_ADDR_SYS] = ADS114S0x._ADS_SENDSTATUS_MASK | ADS114S0x._ADS_CRC_MASK + mock_spi.xfer2.return_value = [0x80, 0x12, 0x34, 0x00, 0xFF] + + with patch.object(adc, "get_crc", return_value=1), pytest.raises(ValueError): + adc.read_converted_data() + + +def test_read_converted_data_command_mode_with_status_byte(adc, mock_spi): + adc._register_map[ADS114S0x._REG_ADDR_SYS] = ADS114S0x._ADS_SENDSTATUS_MASK + # index 0 is the dummy byte clocked out alongside the RDATA opcode + mock_spi.xfer2.return_value = [0x00, 0x80, 0x12, 0x34, 0x00] + + code16, status = adc.read_converted_data(mode=ADS114S0x.ReadMode.COMMAND) + + assert mock_spi.xfer2.call_args[0][0][0] == ADS114S0x._OPCODE_RDATA + assert status == 0x80 + assert code16 == 0x1234 + + +def test_read_converted_data_command_mode_with_status_and_crc(adc, mock_spi): + adc._register_map[ADS114S0x._REG_ADDR_SYS] = ADS114S0x._ADS_SENDSTATUS_MASK | ADS114S0x._ADS_CRC_MASK + mock_spi.xfer2.return_value = [0x00, 0x80, 0x12, 0x34, 0x00, 0xAB] + + with patch.object(adc, "get_crc", return_value=0) as get_crc: + code16, status = adc.read_converted_data(mode=ADS114S0x.ReadMode.COMMAND) + + assert status == 0x80 + assert code16 == 0x1234 + get_crc.assert_called_once_with([0x80, 0x12, 0x34, 0x00, 0xAB], 5) + + +def test_read_converted_data_negative_with_status_byte(adc, mock_spi): + adc._register_map[ADS114S0x._REG_ADDR_SYS] = ADS114S0x._ADS_SENDSTATUS_MASK + mock_spi.xfer2.return_value = [0x40, 0xFF, 0xFF, 0x00] # status, msb, mid, lsb + + code16, status = adc.read_converted_data() + + assert status == 0x40 + assert code16 == -1 + + +def test_read_converted_data_lsb_byte_is_discarded(adc, mock_spi): + # 3 bytes returned, but result is truncated to 16 bits (msb, mid only) + _configure_plain_read(adc) + mock_spi.xfer2.return_value = [0x12, 0x34, 0xFF] # lsb varies + + code16, _ = adc.read_converted_data() + + assert code16 == 0x1234 # lsb has no effect on the returned value + + def test_wait_and_read_code16_success(adc): with ( patch.object(adc, "wait_for_drdy_htol", return_value=True), @@ -553,6 +632,80 @@ def test_get_crc_initializes_table_lazily(adc): assert result == adc._calculate_crc([0x12, 0x34], 2) +def test_init_crc_noop_when_lookup_disabled(adc): + with patch.object(adc, "_CRC_LOOKUP", False), patch.object(adc, "_init_table") as init_table: + adc._initialized = False + adc.init_crc() + + init_table.assert_not_called() + assert adc._initialized is False + + +def test_init_crc_builds_table_when_lookup_enabled(adc): + with patch.object(adc, "_CRC_LOOKUP", True), patch.object(adc, "_init_table") as init_table: + adc.init_crc() + + init_table.assert_called_once() + assert adc._initialized is True + + +def test_get_crc_uses_calculate_when_lookup_disabled(adc): + with ( + patch.object(adc, "_CRC_LOOKUP", False), + patch.object(adc, "_calculate_crc", return_value=0x99) as calc, + patch.object(adc, "_lookup_crc") as lookup, + patch.object(adc, "_init_table") as init_table, + ): + result = adc.get_crc([0x01, 0x02], 2) + + assert result == 0x99 + calc.assert_called_once_with([0x01, 0x02], 2) + lookup.assert_not_called() + init_table.assert_not_called() + + +def test_get_crc_ignores_initialized_flag_when_lookup_disabled(adc): + # Non-lookup mode must never consult _initialized or touch the table. + adc._initialized = False + with ( + patch.object(adc, "_CRC_LOOKUP", False), + patch.object(adc, "_calculate_crc", return_value=0x00) as calc, + patch.object(adc, "_init_table") as init_table, + ): + adc.get_crc([0x01], 1) + + init_table.assert_not_called() + calc.assert_called_once() + + +def test_get_crc_skips_reinit_when_already_initialized(adc): + adc._initialized = True + with ( + patch.object(adc, "_CRC_LOOKUP", True), + patch.object(adc, "_init_table") as init_table, + patch.object(adc, "_lookup_crc", return_value=0x55) as lookup, + ): + result = adc.get_crc([0x01, 0x02], 2) + + init_table.assert_not_called() + lookup.assert_called_once_with([0x01, 0x02], 2) + assert result == 0x55 + + +def test_get_crc_dispatches_to_lookup_not_calculate(adc): + with ( + patch.object(adc, "_CRC_LOOKUP", True), + patch.object(adc, "_lookup_crc", return_value=0x77) as lookup, + patch.object(adc, "_calculate_crc") as calc, + ): + adc._initialized = True + result = adc.get_crc([0xAB], 1) + + lookup.assert_called_once_with([0xAB], 1) + calc.assert_not_called() + assert result == 0x77 + + # adc_configure_common def test_adc_configure_common_bypasses_pga_at_gain_1(adc): adc._pga_gain = 1 From 6d2b8adb42c4959d1706074c4dae62c6654abd8e Mon Sep 17 00:00:00 2001 From: kahei Date: Fri, 14 Aug 2026 14:14:57 -0400 Subject: [PATCH 10/17] fix: mock gpiozero in test_default_constants_no_raise to fix CI failure --- tests/test_actuators/test_brushed.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_actuators/test_brushed.py b/tests/test_actuators/test_brushed.py index 1c7e40ce..24035182 100644 --- a/tests/test_actuators/test_brushed.py +++ b/tests/test_actuators/test_brushed.py @@ -66,8 +66,8 @@ def test_degrees_radians_roundtrip(): # Construction / configuration -def test_default_constants_no_raise(): - actuator = MaxonActuator() +def test_default_constants_no_raise(mock_gpio): + actuator = MaxonActuator(motor_constants=VALID_MOTOR_CONSTANTS) assert actuator is not None From aa9714811b2d32f93fb2938e438493aa78f21e19 Mon Sep 17 00:00:00 2001 From: kahei Date: Fri, 14 Aug 2026 14:58:24 -0400 Subject: [PATCH 11/17] test: add unit tests to improve codecov coverage --- opensourceleg/actuators/brushed.py | 17 +- pyproject.toml | 3 + tests/test_actuators/test_brushed.py | 239 ++++++++++++++++++++++++++- uv.lock | 6 +- 4 files changed, 252 insertions(+), 13 deletions(-) diff --git a/opensourceleg/actuators/brushed.py b/opensourceleg/actuators/brushed.py index dc263417..ad47ebd5 100644 --- a/opensourceleg/actuators/brushed.py +++ b/opensourceleg/actuators/brushed.py @@ -4,7 +4,11 @@ import numpy as np from gpiozero import OutputDevice, PWMOutputDevice -from gpiozero.pins.lgpio import LGPIOFactory + +try: + from gpiozero.pins.lgpio import LGPIOFactory +except ImportError: + LGPIOFactory = None from opensourceleg.actuators.base import ( CONTROL_MODE_CONFIGS, @@ -150,6 +154,13 @@ def __init__( self.pwm_lower_limit = pwm_lower_limit if not self._is_offline: + if LGPIOFactory is None: + LOGGER.error( + "lgpio is not installed. Please install the 'brushed' extra " + "(e.g. `pip install opensourceleg[brushed]`) to use this module on hardware." + ) + exit(1) + self._factory = LGPIOFactory() self.speed_control = PWMOutputDevice(self.enable_pin, frequency=8000, initial_value=0) @@ -531,7 +542,3 @@ def set_motor_encoder(self, encoder_counter: EncoderCounterBase) -> None: encoder_counter: Encoder counter instance providing its attribute. """ self.encoder_counter = encoder_counter - - -if __name__ == "__main__": - pass diff --git a/pyproject.toml b/pyproject.toml index ef2c7d19..861ace6a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -32,6 +32,9 @@ messaging = [ "grpcio-tools>=1.65.5,<2", "types-protobuf>=4.21.0,<5", ] +brushed = [ + "lgpio>=0.2.2.0 ; sys_platform == 'linux' and platform_machine == 'aarch64'", +] [project.urls] Repository = "https://github.com/neurobionics/opensourceleg" diff --git a/tests/test_actuators/test_brushed.py b/tests/test_actuators/test_brushed.py index 24035182..aab50d90 100644 --- a/tests/test_actuators/test_brushed.py +++ b/tests/test_actuators/test_brushed.py @@ -10,6 +10,7 @@ ) from opensourceleg.actuators.brushed import ( MaxonActuator, + _maxon_position_mode_exit, degrees_to_radians, radians_to_degrees, ) @@ -83,6 +84,35 @@ def test_maxon_init(maxon: MaxonActuator): assert maxon.pwm_lower_limit == 0.02 +def test_maxon_init_uses_default_motor_constants(mock_gpio): + """When motor_constants is omitted, MaxonActuator builds its own defaults. + + The base class may store the constants under a private name, so we verify + the defaults via their observable effect: motor_position uses + MOTOR_COUNT_PER_REV=1024, so 1024 encoder counts must equal exactly 2π rad. + """ + encoder = MagicMock() + encoder.count = 1024 + actuator = MaxonActuator() # no motor_constants kwarg + actuator.set_motor_encoder(encoder) + actuator.position_control_config() + assert actuator.motor_position == pytest.approx(2 * np.pi) + + +def test_maxon_init_custom_motor_constants(mock_gpio): + """Explicitly supplied motor constants are stored as-is.""" + custom = MOTOR_CONSTANTS( + MOTOR_COUNT_PER_REV=512, + NM_PER_AMP=0.01, + MAX_CASE_TEMPERATURE=90.0, + MAX_WINDING_TEMPERATURE=130.0, + ) + actuator = MaxonActuator(motor_constants=custom) + mc = actuator.MOTOR_CONSTANTS # base class exposes _MOTOR_CONSTANTS via MOTOR_CONSTANTS property + assert mc.MOTOR_COUNT_PER_REV == 512 + assert pytest.approx(0.01) == mc.NM_PER_AMP + + def test_maxon_control_mode_configs(maxon: MaxonActuator): configs = maxon._CONTROL_MODE_CONFIGS assert configs.POSITION is not None @@ -92,7 +122,24 @@ def test_maxon_control_mode_configs(maxon: MaxonActuator): assert configs.VELOCITY is None -# Unsupported control modes(should raise error) +# Control-mode callbacks +def test_maxon_position_mode_exit_calls_stop(maxon: MaxonActuator): + """_maxon_position_mode_exit must call stop(), zeroing PWM and pins.""" + _maxon_position_mode_exit(maxon) + assert maxon.speed_control.value == 0 + maxon.ina.off.assert_called() + maxon.inb.off.assert_called() + + +def test_maxon_position_mode_exit_via_mode_switch(maxon: MaxonActuator): + """Switching away from POSITION mode should trigger the exit callback.""" + maxon.set_control_mode(CONTROL_MODES.POSITION) + maxon.set_motor_pwm(0.15) # non-zero PWM to confirm stop zeroes it + maxon.set_control_mode(CONTROL_MODES.VOLTAGE) # exit callback fires here + assert maxon.speed_control.value == 0 + + +# Unsupported control modes def test_maxon_voltage_unsupported(maxon: MaxonActuator): maxon.set_control_mode(CONTROL_MODES.VOLTAGE) with pytest.raises(UnsupportedControlModeError): @@ -127,6 +174,12 @@ def test_maxon_impedance_gains_not_implemented(maxon: MaxonActuator): maxon.set_impedance_gains(1.0, 1.0) +# start() +def test_maxon_start_is_noop(maxon: MaxonActuator): + """start() is defined as a no-op; calling it must not raise.""" + maxon.start() # should return None without touching any hardware + + # Gains / PWM / direction def test_maxon_set_position_gains(maxon: MaxonActuator): maxon.set_control_mode(CONTROL_MODES.POSITION) @@ -175,7 +228,7 @@ def test_maxon_stop(maxon: MaxonActuator): maxon.inb.off.assert_called_once() -# Encoder / update / conversions +# Encoder def test_maxon_update_with_encoder(maxon: MaxonActuator): encoder = MagicMock() encoder.count = 5000 @@ -187,6 +240,29 @@ def test_maxon_update_with_encoder(maxon: MaxonActuator): assert maxon.motor_position_perc == maxon.cts_to_perc(5000) +def test_maxon_update_without_encoder_defaults_to_zero(maxon: MaxonActuator): + """update() with no encoder attached must set motor_position_cts to 0.""" + maxon.encoder_counter = None + maxon.position_control_config() + maxon.update() + assert maxon.motor_position_cts == 0.0 + + +def test_maxon_update_reflects_changing_encoder_count(maxon: MaxonActuator): + """Each update() call should pick up the latest encoder count.""" + encoder = MagicMock() + maxon.set_motor_encoder(encoder) + maxon.position_control_config() + + encoder.count = 100 + maxon.update() + assert maxon.motor_position_cts == 100 + + encoder.count = 2048 + maxon.update() + assert maxon.motor_position_cts == 2048 + + def test_maxon_unit_conversions(maxon: MaxonActuator): maxon.position_control_config(scale_perc=100.0, scale=200.0) assert maxon.perc_to_cts(2.0) == 200.0 @@ -217,6 +293,32 @@ def test_maxon_motor_position_property(maxon: MaxonActuator): assert maxon.motor_position == pytest.approx(2 * np.pi) +def test_maxon_motor_encoder_position_perc_calls_update(maxon: MaxonActuator): + """motor_encoder_position_perc must refresh from the encoder on each access.""" + encoder = MagicMock() + maxon.set_motor_encoder(encoder) + maxon.position_control_config(scale_perc=1000.0, scale=2000.0) + + encoder.count = 500 + perc = maxon.motor_encoder_position_perc + assert perc == pytest.approx(maxon.cts_to_perc(500)) + + # Simulate the encoder advancing; the property must reflect the new reading. + encoder.count = 1000 + perc2 = maxon.motor_encoder_position_perc + assert perc2 == pytest.approx(maxon.cts_to_perc(1000)) + assert perc2 != pytest.approx(perc) + + +def test_maxon_motor_encoder_position_perc_at_zero(maxon: MaxonActuator): + """At count 0, the property should return 0.0%.""" + encoder = MagicMock() + encoder.count = 0 + maxon.set_motor_encoder(encoder) + maxon.position_control_config(scale_perc=1000.0, scale=2000.0) + assert maxon.motor_encoder_position_perc == pytest.approx(0.0) + + def test_maxon_unsupported_readings(maxon: MaxonActuator): assert maxon.motor_velocity == 0.0 assert maxon.case_temperature == 0.0 @@ -245,13 +347,11 @@ def test_maxon_pid_returns_float_and_updates_last_pwm(maxon: MaxonActuator): def test_maxon_pid_i_term_integrates_when_not_saturated(maxon: MaxonActuator): - # With the corrected anti-windup guard (0.05 margin instead of 5), # the integrator now updates when last_pwm sits inside the unsaturated window. maxon.set_control_mode(CONTROL_MODES.POSITION) maxon.position_control_init() - # Sanity check on the precondition: last_pwm starts at 0.0, which should - # fall inside the window for any reasonable pwm_maximum_command. + # last_pwm starts at 0.0, which should fall inside the window for any reasonable pwm_maximum_command. assert -(maxon.pwm_maximum_command - 0.05) < maxon.last_pwm < (maxon.pwm_maximum_command - 0.05) maxon.pid_ctrl_position(error_encoder=1000.0, dt=0.01) @@ -271,7 +371,76 @@ def test_maxon_pid_i_term_holds_when_saturated(maxon: MaxonActuator): assert maxon.i_term == 0.0 -# Homing (loop + hardware, with time.sleep patched out) +# check_coupler_drift +def _setup_motor_position_mm(maxon: MaxonActuator, mm: float) -> None: + """Helper: configure scale and plant an encoder count so motor_position_mm == mm.""" + scale = 1000.0 + maxon.position_control_config(scale_perc=1000.0, scale=scale) + encoder = MagicMock() + encoder.count = mm * scale # cts_to_mm(count) == count / scale == mm + maxon.set_motor_encoder(encoder) + maxon.update() + + +def test_check_coupler_drift_no_warning_when_ok(maxon: MaxonActuator): + """Positive mm (normal operating range) must not log any warning.""" + _setup_motor_position_mm(maxon, 5.0) + with patch("opensourceleg.actuators.brushed.LOGGER") as mock_logger: + maxon.check_coupler_drift() + mock_logger.warning.assert_not_called() + + +def test_check_coupler_drift_slight_drift_warning(maxon: MaxonActuator): + """ + When allowable_coupler_drift < position_mm < 0, a 'drifted a little' warning + must be issued (first branch only). + """ + # Default allowable_coupler_drift is -0.2; place position at -0.1. + _setup_motor_position_mm(maxon, -0.1) + with patch("opensourceleg.actuators.brushed.LOGGER") as mock_logger: + maxon.check_coupler_drift() + warning_msgs = [str(c.args[0]) for c in mock_logger.warning.call_args_list] + assert any("drifted a little" in m for m in warning_msgs) + # The severe 'reassembled' warning must NOT appear. + assert not any("reassembled" in m for m in warning_msgs) + + +def test_check_coupler_drift_severe_drift_warning(maxon: MaxonActuator): + """ + When position_mm <= allowable_coupler_drift, the 'should be reassembled' warning + must be issued (second branch). + """ + # Default allowable_coupler_drift is -0.2; place position at exactly -0.2. + _setup_motor_position_mm(maxon, -0.2) + with patch("opensourceleg.actuators.brushed.LOGGER") as mock_logger: + maxon.check_coupler_drift() + warning_msgs = [str(c.args[0]) for c in mock_logger.warning.call_args_list] + assert any("reassembled" in m for m in warning_msgs) + + +def test_check_coupler_drift_severe_drift_well_below(maxon: MaxonActuator): + """Values well below the threshold also trigger the severe warning.""" + _setup_motor_position_mm(maxon, -5.0) + with patch("opensourceleg.actuators.brushed.LOGGER") as mock_logger: + maxon.check_coupler_drift() + warning_msgs = [str(c.args[0]) for c in mock_logger.warning.call_args_list] + assert any("reassembled" in m for m in warning_msgs) + + +def test_check_coupler_drift_boundary_just_above_allowable(maxon: MaxonActuator): + """ + A value just above allowable_coupler_drift but still negative should trigger + the 'drifted a little' warning only. + """ + _setup_motor_position_mm(maxon, -0.15) # -0.2 < -0.15 < 0 + with patch("opensourceleg.actuators.brushed.LOGGER") as mock_logger: + maxon.check_coupler_drift() + warning_msgs = [str(c.args[0]) for c in mock_logger.warning.call_args_list] + assert any("drifted a little" in m for m in warning_msgs) + assert not any("reassembled" in m for m in warning_msgs) + + +# Homing def test_maxon_home_stops_when_stable(maxon: MaxonActuator): encoder = MagicMock() encoder.count = 100 # constant -> position immediately "stable" -> one loop pass @@ -294,7 +463,63 @@ def test_maxon_home_invokes_callback(maxon: MaxonActuator): callback.assert_called_once() -# Offline integration (mirrors the DephyActuator offline example) +def test_maxon_home_zero_sets_direction_backward(maxon: MaxonActuator): + """home(home_zero=True) must drive the motor backward.""" + encoder = MagicMock() + encoder.count = 0 + maxon.set_motor_encoder(encoder) + maxon.position_control_config() + with patch("opensourceleg.actuators.brushed.time.sleep"): + maxon.home(home_zero=True) + # Backward: ina off, inb on (then stop sets both off — check inb.on was called) + maxon.inb.on.assert_called() + + +def test_maxon_home_hardstop_sets_direction_forward(maxon: MaxonActuator): + """home(home_zero=False) must drive the motor forward.""" + encoder = MagicMock() + encoder.count = 0 + maxon.set_motor_encoder(encoder) + maxon.position_control_config() + with patch("opensourceleg.actuators.brushed.time.sleep"): + maxon.home(home_zero=False) + maxon.ina.on.assert_called() + + +def test_maxon_home_no_callback_does_not_raise(maxon: MaxonActuator): + """home() with no callback should complete cleanly.""" + encoder = MagicMock() + encoder.count = 0 + maxon.set_motor_encoder(encoder) + maxon.position_control_config() + with patch("opensourceleg.actuators.brushed.time.sleep"): + maxon.home(callback=None) # must not raise + + +def test_maxon_home_exits_after_position_stabilises(maxon: MaxonActuator): + """ + Simulate a motor that moves for two samples then stops. + home() exits when two consecutive encoder readings differ by less than + position_threshold, and stop() must be called (speed_control.value == 0). + + The encoder count sequence is driven via a closure attached to a + unittest.mock.PropertyMock so that attribute access (not a call) returns + the right value at each update() call. + """ + from unittest.mock import PropertyMock + + encoder = MagicMock() + type(encoder).count = PropertyMock(side_effect=[0, 500, 505, 508, 508]) + + maxon.set_motor_encoder(encoder) + maxon.position_control_config() + with patch("opensourceleg.actuators.brushed.time.sleep"): + maxon.home(position_threshold=10) + + assert maxon.speed_control.value == 0 + + +# Offline integration @pytest.fixture def maxon_offline(): # No gpiozero mocking needed: offline mode skips the hardware branch entirely. diff --git a/uv.lock b/uv.lock index 2417323d..592ec65e 100644 --- a/uv.lock +++ b/uv.lock @@ -967,6 +967,9 @@ bno055 = [ { name = "adafruit-circuitpython-lis3dh", marker = "platform_machine == 'aarch64' and sys_platform == 'linux'" }, { name = "board" }, ] +brushed = [ + { name = "lgpio", marker = "platform_machine == 'aarch64' and sys_platform == 'linux'" }, +] communication = [ { name = "smbus2" }, { name = "spidev", marker = "sys_platform == 'linux'" }, @@ -1024,6 +1027,7 @@ requires-dist = [ { name = "grpcio", marker = "extra == 'messaging'", specifier = ">=1.65.5,<2" }, { name = "grpcio-tools", marker = "extra == 'messaging'", specifier = ">=1.65.5,<2" }, { name = "lgpio", specifier = ">=0.2.2.0" }, + { name = "lgpio", marker = "platform_machine == 'aarch64' and sys_platform == 'linux' and extra == 'brushed'", specifier = ">=0.2.2.0" }, { name = "moteus", marker = "extra == 'moteus'", specifier = ">=0.3.72,<0.4" }, { name = "moteus-pi3hat", marker = "platform_machine == 'aarch64' and sys_platform == 'linux' and extra == 'moteus'", specifier = ">=0.3.29,<0.4" }, { name = "numpy", specifier = ">=1.24.3,<2" }, @@ -1032,7 +1036,7 @@ requires-dist = [ { name = "spidev", marker = "sys_platform == 'linux' and extra == 'communication'", specifier = ">=3.7" }, { name = "types-protobuf", marker = "extra == 'messaging'", specifier = ">=4.21.0,<5" }, ] -provides-extras = ["dephy", "bno055", "moteus", "communication", "messaging"] +provides-extras = ["dephy", "bno055", "moteus", "communication", "messaging", "brushed"] [package.metadata.requires-dev] all = [ From 18d7e2de052494179c6d334e9a19f43a1fe1b300 Mon Sep 17 00:00:00 2001 From: kahei Date: Fri, 14 Aug 2026 15:04:13 -0400 Subject: [PATCH 12/17] fix: update pyproject.toml to fix lgpio denpendency issue --- pyproject.toml | 2 +- uv.lock | 8 ++------ 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 861ace6a..be01a899 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -7,7 +7,7 @@ requires-python = ">=3.11,<4.0" readme = "README.md" dependencies = [ "gpiozero>=2.0.1.post3", - "lgpio>=0.2.2.0", + "lgpio>=0.2.2.0 ; sys_platform == 'linux' and platform_machine == 'aarch64'", "numpy>=1.24.3,<2", "pandas>=2.2.3,<3", ] diff --git a/uv.lock b/uv.lock index 592ec65e..0dcf7d8a 100644 --- a/uv.lock +++ b/uv.lock @@ -646,12 +646,8 @@ version = "0.2.2.0" source = { registry = "https://pypi.org/simple" } sdist = { url = "https://files.pythonhosted.org/packages/56/33/26ec2e8049eaa2f077bf23a12dc61ca559fbfa7bea0516bf263d657ae275/lgpio-0.2.2.0.tar.gz", hash = "sha256:11372e653b200f76a0b3ef8a23a0735c85ec678a9f8550b9893151ed0f863fff", size = 90087, upload-time = "2024-03-29T21:59:55.901Z" } wheels = [ - { url = "https://files.pythonhosted.org/packages/78/4e/5721ae44b29e4fe9175f68c881694e3713066590739a7c87f8cee2835c25/lgpio-0.2.2.0-cp311-cp311-linux_armv7l.whl", hash = "sha256:5b3c403e1fba9c17d178f1bde102726c548fc5c4fc1ccf5ec3e18f3c08e07e04", size = 382992, upload-time = "2024-03-29T22:00:45.039Z" }, { url = "https://files.pythonhosted.org/packages/88/53/e57a22fe815fc68d0991655c1105b8ed872a68491d32e4e0e7d10ffb5c4d/lgpio-0.2.2.0-cp311-cp311-manylinux_2_34_aarch64.whl", hash = "sha256:a2f71fb95b149d8ac82c7c6bae70f054f6dc42a006ad35c90c7d8e54921fbcf4", size = 364848, upload-time = "2024-04-01T22:49:45.889Z" }, - { url = "https://files.pythonhosted.org/packages/a4/71/11f4e3d76400e4ca43f9f9b014f5a86d9a265340c0bea45cce037277eb34/lgpio-0.2.2.0-cp311-cp311-manylinux_2_34_x86_64.whl", hash = "sha256:e9f4f3915abe5ae0ffdb4b96f485076d80a663876d839e2d3fd9218a71b9873e", size = 370183, upload-time = "2024-04-13T14:08:14.139Z" }, - { url = "https://files.pythonhosted.org/packages/fe/73/e56c9afb845df53492d42bdea01df9895272bccfdd5128f34719c3a07990/lgpio-0.2.2.0-cp312-cp312-linux_armv7l.whl", hash = "sha256:6c65ac42e878764d04a71ed12fe6d46089b36e9e8127722bf29bb2e4bc91de22", size = 383956, upload-time = "2024-03-29T22:00:47.315Z" }, { url = "https://files.pythonhosted.org/packages/3b/1c/becd00f66d2c65feed9a668ff9d91732394cb6baba7bec505d55de0e30c9/lgpio-0.2.2.0-cp312-cp312-manylinux_2_34_aarch64.whl", hash = "sha256:d907db79292c721c605af08187385ddb3b7af09907e1ffca56cf0cd6558ace0a", size = 366058, upload-time = "2024-04-01T22:49:47.615Z" }, - { url = "https://files.pythonhosted.org/packages/4c/7a/e3b4e5225c9792c4092b2cc07504746acbe62d0a8e4cb023bdf65f6430cf/lgpio-0.2.2.0-cp312-cp312-manylinux_2_34_x86_64.whl", hash = "sha256:2aadff092f642fcdada8457c158f87259dfda3a89ec19bae0b99ff22b34aac4b", size = 372103, upload-time = "2024-04-13T14:08:16.351Z" }, ] [[package]] @@ -956,7 +952,7 @@ version = "3.5.0" source = { editable = "." } dependencies = [ { name = "gpiozero" }, - { name = "lgpio" }, + { name = "lgpio", marker = "platform_machine == 'aarch64' and sys_platform == 'linux'" }, { name = "numpy" }, { name = "pandas" }, ] @@ -1026,7 +1022,7 @@ requires-dist = [ { name = "gpiozero", specifier = ">=2.0.1.post3" }, { name = "grpcio", marker = "extra == 'messaging'", specifier = ">=1.65.5,<2" }, { name = "grpcio-tools", marker = "extra == 'messaging'", specifier = ">=1.65.5,<2" }, - { name = "lgpio", specifier = ">=0.2.2.0" }, + { name = "lgpio", marker = "platform_machine == 'aarch64' and sys_platform == 'linux'", specifier = ">=0.2.2.0" }, { name = "lgpio", marker = "platform_machine == 'aarch64' and sys_platform == 'linux' and extra == 'brushed'", specifier = ">=0.2.2.0" }, { name = "moteus", marker = "extra == 'moteus'", specifier = ">=0.3.72,<0.4" }, { name = "moteus-pi3hat", marker = "platform_machine == 'aarch64' and sys_platform == 'linux' and extra == 'moteus'", specifier = ">=0.3.29,<0.4" }, From 739d61dcf131c9ec199d770ef8f586dbc2146017 Mon Sep 17 00:00:00 2001 From: anushka Date: Fri, 14 Aug 2026 15:19:15 -0400 Subject: [PATCH 13/17] feat: add vso robot class --- opensourceleg/robots/vso.py | 248 ++++++++++++++++++++++++++++++++++++ 1 file changed, 248 insertions(+) create mode 100644 opensourceleg/robots/vso.py diff --git a/opensourceleg/robots/vso.py b/opensourceleg/robots/vso.py new file mode 100644 index 00000000..aeb4deef --- /dev/null +++ b/opensourceleg/robots/vso.py @@ -0,0 +1,248 @@ +import os +import time +from typing import Callable, Optional, Union, cast + +import numpy as np + +from opensourceleg.actuators.base import CONTROL_MODES, ActuatorBase +from opensourceleg.actuators.brushed import MaxonActuator +from opensourceleg.logging import LOGGER +from opensourceleg.robots.base import RobotBase, TActuator, TSensor +from opensourceleg.sensors.base import SensorBase + + +class VSO(RobotBase[TActuator, TSensor]): + """ + Variable Stiffness Orthosis (VSO) class derived from RobotBase. + """ + + def start(self) -> None: + """Start the VSO.""" + super().start() + + def stop(self) -> None: + """Stop the VSO.""" + super().stop() + + def update(self) -> None: + """Update the VSO.""" + super().update() + + def home( + self, + homing_pwm: float = 0.25, + sample_rate: float = 0.05, + position_threshold: int = 200, + home_zero: bool = True, + timeout_s: float = 8.0, + callbacks: Optional[dict[str, Callable]] = None, + ) -> None: + """ + Call the home method for all motors. + + Args: + homing_pwm: The pwm to apply to the motors during homing. + sample_rate: The time slept between each move during homing. + position_threshold: Minimum error between the last encoder movement and now to stop homing. + Default is 200 for the motor but may want to change if flex coupler attached. + home_zero: Determines the direction of homing. Should be True to move to 0% stiffness and + false to move to 100% stiffness. + callbacks Optional[dict[str, Callable]]: + Optional dictionary of callback functions, one per motor, to be called when each motor's + homing completes. Only one callback per motor is supported, and the tag must match. + Each function should take no arguments and return None. If None, no callbacks are used. + """ + + LOGGER.info("Starting VSO homing routine.") + + for actuator in self.actuators.values(): + maxon = cast(MaxonActuator, actuator) + callback = callbacks.get(actuator.tag, None) if callbacks is not None else None + maxon.home( + homing_pwm=homing_pwm, + sample_rate=sample_rate, + position_threshold=position_threshold, + home_zero=home_zero, + callback=callback, + timeout_s=timeout_s, + ) + + LOGGER.info("VSO homing complete. Spring-support is at hard stop.") + + def make_encoder_linearization_map( + self, + overwrite: bool = False, + ) -> None: + """ + This method makes a lookup table to calculate the position measured by the joint encoder. + + This method is necessary because the magnetic output encoders are nonlinear. By making the map while + the joint is unloaded, joint position calculated by motor position * gear ratio should be the same as + the true joint position. Output from this function is a file containing a_i values parameterizing the map. + + Eqn: + position = sum from i=0^5 (a_i*counts^i) + + Args: + overwrite: If True, regenerate and overwrite any existing linearization map file. + If False, load the existing file when one is found. + """ + for actuator_key in self.actuators: + if f"joint_encoder_{actuator_key}" in self.sensors: + self._create_linear_joint_mapping( + actuator_key=actuator_key, + encoder_key=f"joint_encoder_{actuator_key}", + overwrite=overwrite, + ) + else: + LOGGER.warning( + f"[{actuator_key}] No joint encoder found. Skipping. " + f"Encoder tags should be of the form 'joint_encoder_{actuator_key}'." + ) + + def _create_linear_joint_mapping( + self, + actuator_key: str, + encoder_key: str, + overwrite: bool = False, + ) -> None: + """ + Fit and save a polynomial linearization map for a single joint encoder. + + Moves the actuator into position-control mode, prompts the user to manually sweep the joint through its + full range of motion for 10 seconds, then solves a least-squares polynomial fit between raw encoder + counts and motor-derived output position. The resulting coefficients are saved to + ``_linearization_map.npy``. + + Args: + actuator_key: Key for self.actuators, identifying the joint's actuator. + encoder_key: Key for self.sensors identifying the joint's output encoder. + overwrite: If True, regenerate the map even when a saved file already exists. + """ + _actuator = cast(MaxonActuator, self.actuators[actuator_key]) + _encoder: SensorBase = self.sensors[encoder_key] + + if not _actuator.is_homed: + LOGGER.warning( + msg=f"[{str.upper(_actuator.tag)}] Please home the {_actuator.tag} joint before making the encoder map." + ) + return None + + if os.path.exists(f"./{_encoder.tag}_linearization_map.npy") and not overwrite: + LOGGER.info(msg=f"[{str.upper(_encoder.tag)}] Encoder map exists. Skipping encoder map creation.") + _encoder.set_encoder_map( # type: ignore[attr-defined] + np.polynomial.polynomial.Polynomial(np.load(f"./{_encoder.tag}_linearization_map.npy")) + ) + LOGGER.info( + msg=f"[{str.upper(_encoder.tag)}] Encoder map loaded from './{_encoder.tag}_linearization_map.npy'." + ) + return None + + _actuator.set_control_mode(mode=CONTROL_MODES.POSITION) + _actuator.set_position_gains(0.015, 2, 0.001, 0.0) # default value for position gain + + time.sleep(0.1) + + _joint_encoder_array = [] + _output_position_array = [] + + LOGGER.info( + msg=f"[{str.upper(_actuator.tag)}] Please manually move the {_actuator.tag} joint numerous times through " + f"its full range of motion for 10 seconds." + ) + input("Press any key when you are ready to start.") + + _start_time: float = time.time() + + # TODO: Switch to SoftRealtimeLoop since it has reset method now + while time.time() - _start_time < 10: + try: + LOGGER.info( + msg=f"[{str.upper(_actuator.tag)}] Mapping the {_actuator.tag} " + f"joint encoder: {(10 - time.time() + _start_time):.2f} seconds left." + ) + _actuator.update() + _encoder.update() + + _joint_encoder_array.append(_encoder.position) # type: ignore[attr-defined] + _output_position_array.append(_actuator.output_position) + time.sleep(1 / _actuator.frequency) + + except KeyboardInterrupt: + LOGGER.warning(msg="Encoder map interrupted.") + return None + + LOGGER.info(msg=f"[{str.upper(_actuator.tag)}] You may now stop moving the {_actuator.tag} joint.") + + _power = np.arange(4.0) + _a_mat = np.array(_joint_encoder_array).reshape(-1, 1) ** _power + _beta = np.linalg.lstsq(_a_mat, _output_position_array, rcond=None) + _coeffs = _beta[0] + + _encoder.set_encoder_map(np.polynomial.polynomial.Polynomial(coef=_coeffs)) # type: ignore[attr-defined] + + np.save(file=f"./{_encoder.tag}_linearization_map.npy", arr=_coeffs) + + LOGGER.info( + msg=f"[{str.upper(_encoder.tag)}] Encoder map saved to './{_encoder.tag}_linearization_map.npy' and loaded." + ) + + @property + def ankle(self) -> Union[TActuator, ActuatorBase]: + """ + Get the ankle actuator. + + Returns: + Union[TActuator, ActuatorBase]: The ankle actuator. + + Raises: + KeyError: If no actuator with tag ``ankle`` is registered. + """ + try: + return self.actuators["ankle"] + except KeyError: + LOGGER.error("Ankle actuator not found. Please check for `ankle` key in the actuators dictionary.") + exit(1) + + @property + def joint_encoder_ankle(self) -> Union[TSensor, SensorBase]: + """ + Get the ankle joint encoder sensor. + + Returns: + Union[TSensor, SensorBase]: The ankle joint encoder sensor. + + Raises: + KeyError: If no sensor with tag ``joint_encoder_ankle`` is registered. + """ + try: + return self.sensors["joint_encoder_ankle"] + except KeyError: + LOGGER.error( + "Ankle joint encoder sensor not found." + "Please check for `joint_encoder_ankle` key in the sensors dictionary." + ) + exit(1) + + +if __name__ == "__main__": + frequency = 200 + + vso = VSO[MaxonActuator, SensorBase]( + tag="VariableStiffnessOrthosis", + actuators={ + "ankle": MaxonActuator(tag="ankle", offline=False, frequency=frequency), + }, + sensors=dict[str, SensorBase](), + ) + + with vso: + vso.update() + + while True: + try: + vso.update() + time.sleep(1 / frequency) + + except KeyboardInterrupt: + exit() From 9b34d02f4cb97163ae5d92b70c8956f39773f302 Mon Sep 17 00:00:00 2001 From: anushka Date: Fri, 14 Aug 2026 15:23:14 -0400 Subject: [PATCH 14/17] test: add unitTest for vso robot class --- tests/test_robots/test_vso.py | 336 ++++++++++++++++++++++++++++++++++ 1 file changed, 336 insertions(+) create mode 100644 tests/test_robots/test_vso.py diff --git a/tests/test_robots/test_vso.py b/tests/test_robots/test_vso.py new file mode 100644 index 00000000..45c85734 --- /dev/null +++ b/tests/test_robots/test_vso.py @@ -0,0 +1,336 @@ +from unittest.mock import Mock, patch + +import numpy as np +import pytest + +from opensourceleg.actuators.base import CONTROL_MODES +from opensourceleg.robots.vso import VSO +from tests.test_actuators.test_actuators_base import MOTOR_CONSTANTS, MockActuator +from tests.test_sensors.test_sensors_base import MockSensor + + +def _motor_constants(): + return MOTOR_CONSTANTS( + MOTOR_COUNT_PER_REV=1000, + NM_PER_AMP=0.1, + MAX_CASE_TEMPERATURE=100.0, + MAX_WINDING_TEMPERATURE=150.0, + ) + + +class MockJointEncoder(MockSensor): + """ + Sensor mock with the two members _create_linear_joint_mapping actually uses: + a `position` that advances on each read (so the least-squares fit gets varied + data) and a `set_encoder_map` to capture the fitted polynomial. + """ + + def __init__(self, tag: str = "joint_encoder_ankle"): + super().__init__(tag=tag) + self._position = 0.0 + self.encoder_map = None + + @property + def position(self): + self._position += 1.0 + return self._position + + def set_encoder_map(self, encoder_map): + self.encoder_map = encoder_map + + +class RecordingActuator(MockActuator): + """ + MockActuator that records the mode-gated calls _create_linear_joint_mapping + makes. Defined as a real subclass so the methods exist at __init__ time and + survive ActuatorBase's method-mutation machinery. + """ + + def __init__(self, *args, **kwargs): + self.calls = [] + super().__init__(*args, **kwargs) + + def set_position_gains(self, kp, ki, kd, ff): + self.calls.append(("set_position_gains", kp, ki, kd, ff)) + + def set_motor_voltage(self, value): + self.calls.append(("set_motor_voltage", value)) + + +# Fixtures +@pytest.fixture +def mock_actuator(): + return MockActuator("ankle", 10, _motor_constants()) + + +@pytest.fixture +def mock_sensor(): + return MockSensor(tag="MockSensor") + + +@pytest.fixture +def vso(mock_actuator, mock_sensor): + return VSO( + tag="test_vso", + actuators={"ankle": mock_actuator}, + sensors={"sensor1": mock_sensor}, + ) + + +# Construction and RobotBase plumbing +def test_vso_init(vso: VSO, mock_actuator: MockActuator, mock_sensor: MockSensor): + assert vso.tag == "test_vso" + assert vso.actuators == {"ankle": mock_actuator} + assert vso.sensors == {"sensor1": mock_sensor} + + +def test_vso_enter_calls_start(vso: VSO): + vso.start = Mock() + assert vso.__enter__() == vso + vso.start.assert_called_once() + + +def test_vso_exit_calls_stop(vso: VSO): + vso.stop = Mock() + vso.__exit__(None, None, None) + vso.stop.assert_called_once() + + +def test_vso_start(vso: VSO, mock_actuator: MockActuator, mock_sensor: MockSensor): + mock_actuator.start = Mock() + mock_sensor.start = Mock() + vso.start() + mock_actuator.start.assert_called_once() + mock_sensor.start.assert_called_once() + + +def test_vso_stop(vso: VSO, mock_actuator: MockActuator, mock_sensor: MockSensor): + mock_actuator.stop = Mock() + mock_sensor.stop = Mock() + vso.stop() + mock_actuator.stop.assert_called_once() + mock_sensor.stop.assert_called_once() + + +def test_vso_update(vso: VSO, mock_actuator: MockActuator, mock_sensor: MockSensor): + mock_actuator.update = Mock() + mock_sensor.update = Mock() + vso.update() + mock_actuator.update.assert_called_once() + mock_sensor.update.assert_called_once() + + +# home() +def test_vso_home_defaults(vso: VSO, mock_actuator: MockActuator): + mock_actuator.home = Mock() + vso.home() + mock_actuator.home.assert_called_once_with( + homing_pwm=0.25, + sample_rate=0.05, + position_threshold=200, + home_zero=True, + callback=None, + timeout_s=8.0, + ) + + +def test_vso_home_custom_params(vso: VSO, mock_actuator: MockActuator): + mock_actuator.home = Mock() + vso.home( + homing_pwm=0.1, + sample_rate=0.01, + position_threshold=50, + home_zero=False, + timeout_s=2.0, + ) + mock_actuator.home.assert_called_once_with( + homing_pwm=0.1, + sample_rate=0.01, + position_threshold=50, + home_zero=False, + callback=None, + timeout_s=2.0, + ) + + +def test_vso_home_passes_matching_callback(vso: VSO, mock_actuator: MockActuator): + mock_actuator.home = Mock() + callback = Mock() + # Callbacks are looked up by actuator.tag, which here matches the dict key. + vso.home(callbacks={"ankle": callback}) + assert mock_actuator.home.call_args.kwargs["callback"] is callback + + +def test_vso_home_callback_keyed_by_tag_not_dict_key(): + # Footgun: callbacks.get(actuator.tag) uses the TAG, but the actuators dict is + # keyed independently. When they diverge the callback silently never fires. + actuator = MockActuator("different_tag", 10, _motor_constants()) + actuator.home = Mock() + robot = VSO(tag="test_vso", actuators={"ankle": actuator}, sensors={}) + robot.home(callbacks={"ankle": Mock()}) + assert actuator.home.call_args.kwargs["callback"] is None + + +def test_vso_home_all_actuators(): + left = MockActuator("left", 10, _motor_constants()) + right = MockActuator("right", 10, _motor_constants()) + left.home = Mock() + right.home = Mock() + robot = VSO(tag="test_vso", actuators={"left": left, "right": right}, sensors={}) + robot.home() + left.home.assert_called_once() + right.home.assert_called_once() + + +def test_vso_home_no_actuators(): + robot = VSO(tag="test_vso", actuators={}, sensors={}) + robot.home() # should not raise + + +# Accessor properties +def test_vso_ankle_property(vso: VSO, mock_actuator: MockActuator): + assert vso.ankle is mock_actuator + + +def test_vso_ankle_missing_exits(): + robot = VSO(tag="test_vso", actuators={}, sensors={}) + # The property logs and calls exit(1), which raises SystemExit. + with pytest.raises(SystemExit): + _ = robot.ankle + + +def test_vso_joint_encoder_ankle_property(): + encoder = MockJointEncoder() + robot = VSO(tag="test_vso", actuators={}, sensors={"joint_encoder_ankle": encoder}) + assert robot.joint_encoder_ankle is encoder + + +def test_vso_joint_encoder_ankle_missing_exits(vso: VSO): + with pytest.raises(SystemExit): + _ = vso.joint_encoder_ankle + + +# make_encoder_linearization_map() dispatch +def test_map_skips_actuator_without_encoder(vso: VSO): + # vso's only sensor is "sensor1", so there's no "joint_encoder_ankle". + with patch.object(VSO, "_create_linear_joint_mapping") as mapper: + vso.make_encoder_linearization_map() + mapper.assert_not_called() + + +def test_map_dispatches_when_encoder_present(mock_actuator: MockActuator): + robot = VSO( + tag="test_vso", + actuators={"ankle": mock_actuator}, + sensors={"joint_encoder_ankle": MockJointEncoder()}, + ) + with patch.object(VSO, "_create_linear_joint_mapping") as mapper: + robot.make_encoder_linearization_map(overwrite=True) + mapper.assert_called_once_with( + actuator_key="ankle", + encoder_key="joint_encoder_ankle", + overwrite=True, + ) + + +def test_map_encoder_key_built_from_dict_key_not_tag(): + # The encoder lookup uses the actuators dict key, so a mismatched tag is fine + # here (unlike callbacks in home()). + actuator = MockActuator("some_other_tag", 10, _motor_constants()) + robot = VSO( + tag="test_vso", + actuators={"ankle": actuator}, + sensors={"joint_encoder_ankle": MockJointEncoder()}, + ) + with patch.object(VSO, "_create_linear_joint_mapping") as mapper: + robot.make_encoder_linearization_map() + mapper.assert_called_once() + + +# _create_linear_joint_mapping() +def test_mapping_aborts_when_not_homed(mock_actuator: MockActuator): + encoder = MockJointEncoder() + robot = VSO( + tag="test_vso", + actuators={"ankle": mock_actuator}, + sensors={"joint_encoder_ankle": encoder}, + ) + assert mock_actuator.is_homed is False + robot._create_linear_joint_mapping("ankle", "joint_encoder_ankle") + # Bails before touching the encoder or changing modes. + assert encoder.encoder_map is None + assert mock_actuator.mode == CONTROL_MODES.IDLE + + +def test_mapping_loads_existing_file(mock_actuator: MockActuator, monkeypatch, tmp_path): + monkeypatch.chdir(tmp_path) + encoder = MockJointEncoder() + coeffs = np.array([1.0, 2.0, 3.0, 4.0]) + np.save(f"./{encoder.tag}_linearization_map.npy", coeffs) + + mock_actuator._is_homed = True + robot = VSO( + tag="test_vso", + actuators={"ankle": mock_actuator}, + sensors={"joint_encoder_ankle": encoder}, + ) + robot._create_linear_joint_mapping("ankle", "joint_encoder_ankle", overwrite=False) + + # Loaded the saved polynomial and returned early without entering POSITION mode. + assert encoder.encoder_map is not None + assert np.array_equal(encoder.encoder_map.coef, coeffs) + assert mock_actuator.mode == CONTROL_MODES.IDLE + + +def test_mapping_full_run(monkeypatch, tmp_path): + """ + Exercise the full fit path. time is mocked out (the real loop runs for 10 s of + wall clock) and input() is stubbed. The clock advances 0.5 s per call, so the + loop terminates after roughly 10 iterations. + """ + monkeypatch.chdir(tmp_path) + actuator = RecordingActuator("ankle", 10, _motor_constants()) + actuator._is_homed = True + encoder = MockJointEncoder() + robot = VSO( + tag="test_vso", + actuators={"ankle": actuator}, + sensors={"joint_encoder_ankle": encoder}, + ) + + clock = iter([i * 0.5 for i in range(200)]) + with patch("opensourceleg.robots.vso.time") as mock_time, patch("builtins.input"): + mock_time.time.side_effect = lambda: next(clock) + robot._create_linear_joint_mapping("ankle", "joint_encoder_ankle", overwrite=True) + + # Fitted a polynomial, handed it to the encoder, and saved it to disk. + assert encoder.encoder_map is not None + assert (tmp_path / f"{encoder.tag}_linearization_map.npy").exists() + + # Gains are set with the hardcoded position defaults. + assert ("set_position_gains", 0.015, 2, 0.001, 0.0) in actuator.calls + + +def test_mapping_ends_with_unsupported_voltage_command(monkeypatch, tmp_path): + """ + _create_linear_joint_mapping sets position gains then runs the data-collection + loop and saves the map. It does not issue any voltage or PWM command at the end. + The only actuator call made is set_position_gains with the default PID values. + """ + monkeypatch.chdir(tmp_path) + actuator = RecordingActuator("ankle", 10, _motor_constants()) + actuator._is_homed = True + encoder = MockJointEncoder() + robot = VSO( + tag="test_vso", + actuators={"ankle": actuator}, + sensors={"joint_encoder_ankle": encoder}, + ) + + clock = iter([i * 0.5 for i in range(200)]) + with patch("opensourceleg.robots.vso.time") as mock_time, patch("builtins.input"): + mock_time.time.side_effect = lambda: next(clock) + robot._create_linear_joint_mapping("ankle", "joint_encoder_ankle", overwrite=True) + + assert ("set_position_gains", 0.015, 2, 0.001, 0.0) in actuator.calls + assert not any(call[0] == "set_motor_voltage" for call in actuator.calls) From 90323f204764bff9b1607d66fe04cca51d848a6d Mon Sep 17 00:00:00 2001 From: KaHei Date: Fri, 14 Aug 2026 15:26:19 -0400 Subject: [PATCH 15/17] doc: add tutorial to read vso sensors --- tutorials/sensors/reading_vso_sensors.py | 133 +++++++++++++++++++++++ 1 file changed, 133 insertions(+) create mode 100644 tutorials/sensors/reading_vso_sensors.py diff --git a/tutorials/sensors/reading_vso_sensors.py b/tutorials/sensors/reading_vso_sensors.py new file mode 100644 index 00000000..35028128 --- /dev/null +++ b/tutorials/sensors/reading_vso_sensors.py @@ -0,0 +1,133 @@ +import time + +import numpy as np + +from opensourceleg.actuators.brushed import MaxonActuator +from opensourceleg.logging import LOGGER +from opensourceleg.logging.logger import Logger +from opensourceleg.robots.vso import VSO +from opensourceleg.sensors.adc import ADS114S0x, ChannelConfig +from opensourceleg.sensors.base import SensorBase +from opensourceleg.sensors.encoder import AS5048B +from opensourceleg.sensors.hall import DRV5056 +from opensourceleg.utilities.softrealtimeloop import SoftRealtimeLoop + +FREQUENCY = 200 # control/read loop rate (Hz) +READ_FREQUENCY = 10 # console readout rate (Hz) +OFFLINE = False +SIDE = -1 # 1: left-leg lateral encoder (-1 if medial); -1: right-leg lateral (1 if medial) +ENCODER_ALPHA = 0.15 # EMA smoothing factor (~5 Hz cutoff at 200 Hz) + + +def _hall_voltage(adc: ADS114S0x) -> float: + """Return the first ADC channel reading in volts, or 0.0 if unavailable. + + Args: + adc: The ADS114S0x instance whose latest data to read. + + Returns: + float: Hall channel voltage in volts (ADC data is in millivolts). + """ + data = getattr(adc, "_data", None) + if not data: + return 0.0 + return data[0] / 1000.0 + + +def read_sensors(data_logger: Logger) -> None: + """Configure the VSO sensors and stream their readings to the data logger. + + Args: + data_logger: Logger used to record the tracked sensor values to CSV. + """ + # Define the VSO robot (sensors only) + vso = VSO[MaxonActuator, SensorBase]( + tag="variableStiffnessOrthosis", + actuators={}, + sensors={ + "adc": ADS114S0x(offline=OFFLINE, tag="adc", spi_bus=1, data_rate=2000, drdy=16, voltage_reference=1.65), + "hallEffect_sensor": DRV5056( + offline=OFFLINE, tag="hall_effect", sensor_num="A1", t_a=23, supply_voltage=3.3 + ), + "ankle_encoder": AS5048B( + offline=OFFLINE, + tag="joint_encoder_ankle", + bus="/dev/i2c-3", + A1_adr_pin=False, + A2_adr_pin=True, + zero_position=0, + enable_diagnostics=False, + ), + }, + ) + LOGGER.info("Finished setting up VSO.") + + # ADC and Hall channel configuration + adc = vso.sensors.get("adc") + if adc is None: + LOGGER.error("No ADC found — cannot read the Hall effect sensor. Aborting.") + return + + adc.adc_configure_common(single_shot=True, filter_low_latency=True) + + hall = vso.sensors.get("hallEffect_sensor") + if hall is not None: + hall.configure() + adc._channels["hall_drv5056_ain3"] = ChannelConfig( + name="hall_drv5056_ain3", + ain_pos_code=adc._ADS_P_AIN3, # set to the AIN pin the Hall output is wired to + postprocess=None, + units="V", + ) + + # Ankle encoder offset (referenced to the unloaded equilibrium angle) + calib_offset = 0.0 + ankle_sensor = vso.sensors.get("ankle_encoder") + if ankle_sensor is not None: + LOGGER.info("Capturing unloaded equilibrium angle. Waiting for ankle encoder warmup.") + time.sleep(2) # warmup period for the ankle encoder to stabilize + ankle_sensor.update() + calib_offset = SIDE * np.rad2deg(ankle_sensor.position) + LOGGER.info(f"Ankle encoder offset calibrated. calib_offset={calib_offset:.4f} deg") + else: + LOGGER.warning("No ankle encoder found. Angle will not be captured.") + + # Logged values + angle = 0.0 + data_logger.track_function(lambda: angle, name="ankleEncoderPos") + data_logger.track_function(lambda: _hall_voltage(adc), name="hallEffect_data") + + readout_interval = FREQUENCY // READ_FREQUENCY + + with vso: + loop = SoftRealtimeLoop(dt=1 / FREQUENCY) + loop_count = 0 + t_start = time.monotonic() + + for loop_count in enumerate(loop): + vso.update() + + # Ankle angle: raw reading, referenced to calib_offset, then EMA-smoothed. + angle_raw = SIDE * np.rad2deg(vso.sensors["ankle_encoder"].position) - calib_offset + angle = angle_raw if loop_count == 0 else ENCODER_ALPHA * angle_raw + (1 - ENCODER_ALPHA) * angle + + hall_data = _hall_voltage(adc) + + data_logger.update() + data_logger.flush_buffer() + + if loop_count % readout_interval == 0: + elapsed = time.monotonic() - t_start + print( + f"\r t={elapsed:6.1f}s" f" angle={angle:+7.2f}°" f" hall={hall_data:+.4f} V", + end="", + flush=True, + ) + + +if __name__ == "__main__": + data_logger = Logger( + log_path="./logs", + file_name="read_sensor", + ) + read_sensors(data_logger) From 7b4930af3909856b36ee742207eb554804012776 Mon Sep 17 00:00:00 2001 From: anushka Date: Fri, 14 Aug 2026 15:50:34 -0400 Subject: [PATCH 16/17] test: add more unitTest for vso to improve codecov coverage --- tests/test_robots/test_vso.py | 43 ++++++++++++++++++++++++++++++++++- 1 file changed, 42 insertions(+), 1 deletion(-) diff --git a/tests/test_robots/test_vso.py b/tests/test_robots/test_vso.py index 45c85734..e14722a1 100644 --- a/tests/test_robots/test_vso.py +++ b/tests/test_robots/test_vso.py @@ -1,4 +1,4 @@ -from unittest.mock import Mock, patch +from unittest.mock import MagicMock, Mock, patch import numpy as np import pytest @@ -334,3 +334,44 @@ def test_mapping_ends_with_unsupported_voltage_command(monkeypatch, tmp_path): assert ("set_position_gains", 0.015, 2, 0.001, 0.0) in actuator.calls assert not any(call[0] == "set_motor_voltage" for call in actuator.calls) + + +@pytest.fixture +def vso_with_mocks(): + actuator = MagicMock() + actuator.tag = "ankle" + actuator.is_homed = True + actuator.frequency = 200 + actuator.update.side_effect = KeyboardInterrupt + + encoder = MagicMock() + encoder.tag = "joint_encoder_ankle" + + vso = VSO( + tag="VariableStiffnessOrthosis", + actuators={"ankle": actuator}, + sensors={"joint_encoder_ankle": encoder}, + ) + return vso, actuator, encoder + + +def test_create_linear_joint_mapping_keyboard_interrupt(vso_with_mocks): + vso, actuator, encoder = vso_with_mocks + with ( + patch("opensourceleg.robots.vso.os.path.exists", return_value=False), + patch("opensourceleg.robots.vso.time.sleep"), + patch("builtins.input", return_value=""), + patch("opensourceleg.robots.vso.LOGGER") as mock_logger, + patch("opensourceleg.robots.vso.np.save") as mock_save, + ): + result = vso._create_linear_joint_mapping( + actuator_key="ankle", + encoder_key="joint_encoder_ankle", + ) + + assert result is None + mock_logger.warning.assert_called_once_with(msg="Encoder map interrupted.") + actuator.update.assert_called_once() + encoder.update.assert_not_called() # interrupt fires before this line is reached + mock_save.assert_not_called() + encoder.set_encoder_map.assert_not_called() From c400ba955f7a0035d250d9c55290d8e8c5cc31b3 Mon Sep 17 00:00:00 2001 From: ngkahei Date: Fri, 4 Sep 2026 00:31:12 -0400 Subject: [PATCH 17/17] fix: compilation error in reading vso sensors --- tutorials/sensors/reading_vso_sensors.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorials/sensors/reading_vso_sensors.py b/tutorials/sensors/reading_vso_sensors.py index 35028128..33ac290d 100644 --- a/tutorials/sensors/reading_vso_sensors.py +++ b/tutorials/sensors/reading_vso_sensors.py @@ -104,7 +104,7 @@ def read_sensors(data_logger: Logger) -> None: loop_count = 0 t_start = time.monotonic() - for loop_count in enumerate(loop): + for loop_count, _ in enumerate(loop): vso.update() # Ankle angle: raw reading, referenced to calib_offset, then EMA-smoothed.