ESP32-S3玩转步进电机：开环转起来，闭环不丢步

    def _spi_transfer(self, address, data):        # TMC4361 expects 1 byte address + 4 bytes data        write_buffer = bytearray(5)        read_buffer = bytearray(5)

        write_buffer[0] = address        write_buffer[1] = (data >> 24) & 0xFF        write_buffer[2] = (data >> 16) & 0xFF        write_buffer[3] = (data >> 8) & 0xFF        write_buffer[4] = data & 0xFF

        self._cs.value = False  # Select chip        self._spi.try_lock()        self._spi.write_readinto(write_buffer, read_buffer) # Perform full duplex transfer        self._spi.unlock()        self._cs.value = True   # Deselect chip

        self._spi_status = read_buffer[0] # First byte of read_buffer is the status byte

        # Basic error checking based on SPI status byte (refer to TMC4361A datasheet for details)        # This is a simplified check, more detailed error handling can be added.        # if self._spi_status & 0x01: # Check for general error flag (example, actual bit might vary)        #     print(f"Warning: TMC4361 SPI communication error detected. Status: {self._spi_status:02X}")

        # The returned data is the 32-bit value from the chip, starting from the second byte of read_buffer        # 组合32位数据        value = (read_buffer[1] << 24) | (read_buffer[2] << 16) | (read_buffer[3] << 8) | read_buffer[4]
        # 检查最高位是否为1（负数）        if value & 0x80000000:            # 进行符号扩展，将32位有符号整数转换为Python的负数表示            value = value - 0x100000000        return value

# 设置TMC4361的运动参数round = 360 / 1.8 * 16 * 16tmc.set_accelerations(round, round, 0, 0)

# 设置目标位置tmc.set_ramp_mode(TMC4361.TMC4361.RampMode.POSITIONING_MODE, TMC4361.TMC4361.RampType.TRAPEZOIDAL_RAMP)tmc.set_max_speed(round)target = int(round * 3)tmc.set_target_position(target)print("Target position set")

# 设置目标速度tmc.set_ramp_mode(TMC4361.TMC4361.RampMode.VELOCITY_MODE, TMC4361.TMC4361.RampType.TRAPEZOIDAL_RAMP)tmc.set_max_speed(round * -1)print("Target velocity set")

    def initialize_spi_passthrough(self):        """初始化TMC4361以启用与TMC2160的SPI passthrough功能"""        print("正在初始化TMC4361的SPI Passthrough功能...")        # 读取当前的SPI_OUT_CONF值        spi_out_conf = self.read_register(TMC4361_SPI_OUT_CONF_REGISTER)        print(f"当前的 SPI_OUT_CONF: 0x{spi_out_conf:08X}")

        # 设置 spi_output_format 为 13 (b'1101') 以选择TMC2130/2160        # 同时确保 COVER_DATA_LENGTH (bits 23:21) 为 0        spi_out_conf &= ~0x00E0000F # 清除 spi_output_format 和 COVER_DATA_LENGTH 位        spi_out_conf |= 0x0000000C # 设置 spi_output_format = 12        # spi_out_conf = 0x44401280 | 0x0C << 0        print(f"将要写入的 SPI_OUT_CONF: 0x{spi_out_conf:08X}")        self.write_register(TMC4361_SPI_OUT_CONF_REGISTER, spi_out_conf)

        # 再次读取以确认写入成功        new_spi_out_conf = self.read_register(TMC4361_SPI_OUT_CONF_REGISTER)        print(f"确认后的 SPI_OUT_CONF: 0x{new_spi_out_conf:08X}")        if new_spi_out_conf == spi_out_conf:            print("SPI Passthrough 初始化成功！")        else:            print("SPI Passthrough 初始化失败！")

    def spi_passthrough_transfer_v2(self, address, data=None):        """        修正后的SPI passthrough传输方法，增加了前置配置。
        Args:            address: TMC2160寄存器地址 (7位)            data: 要写入的数据 (32位)（None表示读取操作）
        Returns:            读取的数据或状态 (32位)        """        if data is not None:            # 写操作            tmc2160_address_byte = (address | 0x80) & 0xFF

            # 再写入地址到COVER_HIGH，触发传输            self.write_register(TMC4361_COVER_HIGH_REGISTER, tmc2160_address_byte)            # 先写入数据到COVER_LOW            self.write_register(TMC4361_COVER_LOW_REGISTER, data)            time.sleep(0.1)            # 写操作通常返回0或状态，这里简化处理            return 0        else:            # 读操作            tmc2160_address_byte = address & 0x7F


            # 再写入地址到COVER_HIGH，触发传输            self.write_register(TMC4361_COVER_HIGH_REGISTER, tmc2160_address_byte)            # 对于读操作，先向COVER_LOW写入虚拟数据0            self.write_register(TMC4361_COVER_LOW_REGISTER, 0)
            # 等待传输完成            time.sleep(0.1)
            # 读取响应            # 注意：根据数据手册，响应数据在COVER_DRV_LOW中，状态在COVER_DRV_HIGH中            response_data = self.read_register(TMC4361_COVER_DRV_LOW_REGISTER)            response_status = self.read_register(TMC4361_COVER_DRV_HIGH_REGISTER)
            print(f"TMC2160响应 - 状态: 0x{response_status:08X}, 数据: 0x{response_data:08X}")            return response_data

import timefrom adafruit_bus_device.spi_device import SPIDevice

# Register addresses_CNVPAGE = 0x01_ABSANGLE = 0x03_ANGLE = 0x05_GENERAL = 0x10 # General Register, Page 0x02_FAULT = 0x06 # Fault Register, Page Agnostic

# Bit masks for GENERAL register# No direct software reset bit for GMR found in datasheet. Reset is physical.

class ADMT4000:    """Driver for the ADMT4000 multiturn sensor."""

    def __init__(self, spi, cs, baudrate=10000000, polarity=0, phase=0):        self.spi_device = SPIDevice(spi, cs, baudrate=baudrate, polarity=polarity, phase=phase)        self._buffer = bytearray(4)

    def _read_register(self, address):        """Read a 16-bit value from a register."""        # Per datasheet Figure 18 (SPI Register Read Operation):        # The first bit sent by the microcontroller is ignored.        # The second bit is set low (0) to select the read register configuration.        # The 6-bit register address A5 to A0 is then clocked into the ADMT4000.        # This means the first byte of the 32-bit frame should be structured as:        # [Ignored Bit (can be 0)][R/W Bit (0 for read)][A5][A4][A3][A2][A1][A0]        # So, the command byte should be (address & 0x3F).        # The remaining 3 bytes of the command frame are 0x00.        self._buffer[0] = address & 0x3F        self._buffer[1] = 0x00        self._buffer[2] = 0x00        self._buffer[3] = 0x00

        # Perform the 32-bit SPI transaction        with self.spi_device as spi:            spi.write_readinto(self._buffer, self._buffer)

        # The datasheet (Figure 18) indicates the response frame structure:        # Byte 0: [Ignored Bit][R/W Bit (1)][A5][A4][A3][A2][A1][A0] (echo of command, but R/W=1)        # Byte 1: [Data MSB (D15)][D14][D13][D12][D11][D10][D9][D8]        # Byte 2: [Data LSB (D7)][D6][D5][D4][D3][D2][D1][D0]        # Byte 3: [First Bit Following Data (always 1)][C1][C0][CRC4][CRC3][CRC2][CRC1][CRC0]        # The 16-bit data is in self._buffer[1] and self._buffer[2].        return (self._buffer[1] << 8) | self._buffer[2]

    def _write_register(self, address, value):        """Write a 16-bit value to a register."""        # Per datasheet Figure 19 (32-Bit Write Operation for 16-Bit Register):        # The first bit sent by the microcontroller is ignored.        # The second bit is set high (1) for a write.        # The 6-bit register address A5 to A0 is then clocked into the ADMT4000.        # This makes up the first byte of the 32-bit SPI frame.        # Command byte: [Ignored Bit (can be 0)][R/W Bit (1 for write)][A5][A4][A3][A2][A1][A0]        # So, the command byte should be (0x40 | (address & 0x3F)). (0x40 sets the R/W bit to 1)        self._buffer[0] = 0x40 | (address & 0x3F)        self._buffer[1] = (value >> 8) & 0xFF  # MSB of data        self._buffer[2] = value & 0xFF         # LSB of data        self._buffer[3] = 0x00 # The datasheet mentions 3 following bits are not specified, and then 5 bits CRC. For now, send 0.

        with self.spi_device as spi:            spi.write(self._buffer)

    def gmr_reset(self):        """Resets the GMR turn count sensor. This will set the turn count to 45.        Requires a conversion sequence abort and restart after reset.        Note: The GMR sensor reset is a physical action (e.g., applying a strong magnetic field        at a specific angle or over-rotating the magnet). This method handles the software        sequence (abort/restart conversion) that should follow the physical reset.        """        # Abort current conversion sequence        # Set CNVPAGE bits 15:14 to 0b11 to abort        current_cnvpage = self._read_register(_CNVPAGE)        self._write_register(_CNVPAGE, (current_cnvpage | 0xC000)) # Set bits 15:14 to 11        time.sleep(0.01) # Small delay for the device to process

        # Restart the conversion sequence        # Set CNVPAGE bits 15:14 to 0b00 to restart        self._write_register(_CNVPAGE, (current_cnvpage & 0x3FFF)) # Set bits 15:14 to 00        time.sleep(0.01) # Small delay

    def reset_angle_registers(self):        """Resets the ANGLE and ABSANGLE registers.        The datasheet indicates that ANGLE and ABSANGLE are read-only registers.        Their values are derived from sensor measurements. To 'reset' them,        a GMR sensor reset is the primary mechanism, which sets the turn count to 45.        This function will trigger the GMR reset sequence.        """        print("Warning: ANGLE and ABSANGLE registers are read-only. Performing GMR reset to affect turn count.")        self.gmr_reset()

    @property    def angle(self):        """The single-turn angle in degrees."""        raw_angle = self._read_register(_ANGLE)        # The ANGLE register provides the upper 12 bits of the angle data [15:4].        # Angle Resolution = 360°/4096.        # The raw_angle read from the device is already the 16-bit value, where bits [15:4] are angle data.        angle_data = raw_angle >> 4  # Shift right by 4 to get the 12-bit angle value        return (angle_data / 4096.0) * 360.0

    @property    def absolute_angle(self):        """The absolute angle over multiple turns, in degrees."""        raw_abs_angle = self._read_register(_ABSANGLE)        # ABSANGLE[15:10] contains the number of whole turns (6 bits).        # ABSANGLE[9:0] contains the angle information in straight binary with a resolution of 0.351° (10 bits).

        # Extract turn count (bits 15-10)        turn_count_raw = (raw_abs_angle >> 10) & 0x3F  # Mask to ensure 6 bits

        # Extract angle part (bits 9-0)        angle_part_raw = raw_abs_angle & 0x03FF  # Mask for lower 10 bits

        # Handle invalid turn count (0b110110 = 54) as per datasheet Table 11        if turn_count_raw == 0b110110:            return float("nan")

        # Handle two\'s complement for negative turns (0b110111 to 0b111111) as per datasheet Table 11        if turn_count_raw >= 0b110111:            turn_count = turn_count_raw - 64  # Convert 6-bit two\'s complement to negative integer        else:            turn_count = turn_count_raw

        # Calculate absolute angle: turn_count * 360° + angle_part * 0.351°        return (turn_count * 360.0) + (angle_part_raw * 0.351)

    @property    def turn_count(self):        """The number of turns."""        raw_abs_angle = self._read_register(_ABSANGLE)        # ABSANGLE[15:10] contains the number of whole turns (6 bits).        turn_count_raw = (raw_abs_angle >> 10) & 0x3F  # Mask to ensure 6 bits

        # Handle invalid turn count        if turn_count_raw == 0b110110:            return None

        # Handle two\'s complement for negative turns        if turn_count_raw >= 0b110111:            return turn_count_raw - 64        else:            return turn_count_raw