wire_controlsystem/conveyor_controller/conveyor_motor.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
# @Time    : 2026/2/10 09:42
# @Author  : reenrr
# @File    : conveyor_motor.py
# @Desc    : 传送带电机相关接口   需要测试
"""
import time

from modbus import (write_single_register, open_serial_port, RTU_HANDLE_MAP, RTU_HANDLE_LOCK, read_holding_register)
from error_code import ModbusError

PORT = '/dev/ttyUSB0'
BAUDRATE = 115200
DATABITS = 8
STOPBITS = 1
PARITY = 0

# --------寄存器地址---------
MODE_REG_ADDR = 0x6200
POSITION_H_REG_ADDR = 0x6201
POSITION_L_REG_ADDR = 0x6202
SPEED_REG_ADDR = 0x6203
ACCELERATION_REG_ADDR = 0x6204
DECELERATION_REG_ADDR = 0x6205
START_OR_STOP_REG_ADDR = 0x6002
ENCODER_H_REG_ADDR = 0x0B1C                # 读取电机位置--编码器单位
COMMAND_H_REG_ADDR = 0x602C                # 读电机位置--指令单位
POSITION_ERROR_H_REG_ADDR = 0x0B1E         # 位置误差


# ---------相关写入值------------
START_CMD = 0x0010                  # 启动指令
STOP_CMD = 0x0040                   # 停止指令
SPEED_CMD = 0x0002                  # 速度指令
ABSOLUTE_POSITION_CMD = 0x0001      # 绝对位置指令 
RELATIVE_POSITION_CMD = 0x0041      # 相对位置指令

handle1 = open_serial_port(
    port=PORT,
    baudrate=BAUDRATE,
    databits=DATABITS,
    stopbits=STOPBITS,
    parity=PARITY
)

def _check_handle_valid():
    """通用句柄校验函数"""
    if handle1 is None:
        err_msg = ModbusError.get_error_desc(ModbusError.MODBUS_ERR_SERIAL_HANDLE)
        print(f"错误：{err_msg}")
        raise RuntimeError(err_msg)

    with RTU_HANDLE_LOCK:
        handle_obj = RTU_HANDLE_MAP.get(handle1)
        if not handle_obj or not handle_obj.is_open:
            err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_SERIAL_HANDLE)}（句柄{handle1}）"
            print(f"错误：{err_msg}")
            raise RuntimeError(err_msg)


def set_motor_position(station_addr: int, position_value: int) -> bool:
    """
    设置伺服电机位置
    :param station_addr: 从机地址
    :param position_value: 位置值（32位整数）   正数：远离电机方向；负数：靠近电机方向
    :return: True--设置成功，False--设置失败
    """
    _check_handle_valid()

    try:
        # 有符号32位位置值拆分为高16位和低16位
        position_value = position_value & 0xFFFFFFFF    # 确保为32位无符号整数
        position_h = (position_value >> 16) & 0xFFFF    # 高16位
        position_l = position_value & 0xFFFF            # 低16位
        print(f"位置值拆分：{position_value} → 高16位:(0x{position_h:04x}) 低16位:(0x{position_l:04x})")

    except Exception as e:
        err_msg = f"位置值拆分失败 - {str(e)}"
        print(f"错误：{err_msg}")
        raise ValueError(err_msg) from e

    try:
        # 写入高16位到0x6201
        ret_code_h = write_single_register(handle1, station_addr, POSITION_H_REG_ADDR,
                                           position_h, resp_offset=0, use_crc=1)
        if ret_code_h != ModbusError.MODBUS_SUCCESS:
            err_desc = ModbusError.get_error_desc(ret_code_h)
            raise OSError(f"位置高位写入失败：{err_desc}", ret_code_h)

        # 写入低16位到 0x6202
        ret_code_l = write_single_register(handle1, station_addr, POSITION_L_REG_ADDR,
                                           position_l, resp_offset=0, use_crc=1)
        if ret_code_l != ModbusError.MODBUS_SUCCESS:
            err_desc = ModbusError.get_error_desc(ret_code_l)
            raise OSError(f"位置低位写入失败：{err_desc}", ret_code_l)
        
        print(f"成功：电机位置设置完成（从站{station_addr}，位置值:{position_value}）")
        return True

    except OSError:
        raise
    except Exception as e:
        err_msg = f"位置设置异常 - {str(e)}"
        print(f"异常：{err_msg}")
        raise Exception(err_msg) from e


def set_motor_speed(station_addr: int, speed_value: int) -> bool:
    """
    设置伺服电机速度
    :param station_addr: 从机地址
    :param speed_value: 速度值     速度模式时，正负值表示方向，正值--远离电机方向，负值--靠近电机方向
                                  其他模式时，速度没有方向概念，只表示速度大小
    :return: True--设置成功，False--设置失败
    """
    _check_handle_valid()

    handle_obj = RTU_HANDLE_MAP.get(handle1)

    try:
        value = handle_obj.decimal_to_16bit(speed_value)
        # print(f"速度值转换：{speed_value} → {value}（0x{value:04X}）")

    except Exception as e:
        err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_DATA_CONVERT)} - {str(e)}"
        print(f"错误：{err_msg}")
        raise ValueError(err_msg) from e  # 转换失败抛ValueError，保留原始异常栈

    try:
        ret_code = write_single_register(handle1, station_addr, SPEED_REG_ADDR, value, 0, 1)

        if ret_code == ModbusError.MODBUS_SUCCESS:
            print(f"成功：电机速度设置完成（从站{station_addr}，地址0x{SPEED_REG_ADDR:04X}，值{value}）")
            return True

        else:
            # 写寄存器返回错误码：抛OSError
            err_desc = ModbusError.get_error_desc(
                ret_code) if ret_code in ModbusError.__members__.values() else f"未知错误码{ret_code}"
            err_msg = f"速度设置失败，{err_desc}"
            print(f"失败：{err_msg}")
            raise OSError(err_msg, ret_code)  # 携带错误码，便于上层解析

    except OSError:
        raise
    except Exception as e:
        err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_REG_WRITE)} - {str(e)}"
        print(f"异常：{err_msg}")
        raise Exception(err_msg) from e


def set_motor_acceleration(station_addr: int, value: int) -> bool:
    """
    设置伺服电机加速度
    :param station_addr: 从机地址
    :param value: 加速度值
    :return: True--设置成功，False--设置失败
    """
    _check_handle_valid()

    handle_obj = RTU_HANDLE_MAP.get(handle1)

    try:
        conv_value = handle_obj.decimal_to_16bit(value)
        # print(f"加速度值转换：{value} → {conv_value}（0x{conv_value:04X}）")

    except Exception as e:
        err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_DATA_CONVERT)} - {str(e)}"
        print(f"错误：{err_msg}")
        raise ValueError(err_msg) from e

    try:
        ret_code = write_single_register(handle1, station_addr, ACCELERATION_REG_ADDR, conv_value, 0, 1)

        if ret_code == ModbusError.MODBUS_SUCCESS:
            print(f"成功：电机加速度设置完成（从站{station_addr}，地址0x{ACCELERATION_REG_ADDR:04X}，值{conv_value}）")
            return True

        else:
            # 写寄存器返回错误码：抛OSError（携带错误码）
            err_desc = ModbusError.get_error_desc(
                ret_code) if ret_code in ModbusError.__members__.values() else f"未知错误码{ret_code}"
            err_msg = f"加速度设置失败，{err_desc}"
            print(f"失败：{err_msg}")
            raise OSError(err_msg, ret_code)  # 错误码存入args，上层可解析

    except OSError:
        # 主动抛出的OSError直接上抛，不做额外处理
        raise

    except Exception as e:
        # 其他未知异常：抛通用Exception，保留原始栈
        err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_REG_WRITE)} - {str(e)}"
        print(f"异常：{err_msg}")
        raise Exception(err_msg) from e


def set_motor_deceleration(station_addr: int, value: int) -> bool:
    """
    设置伺服电机减速度
    :param station_addr: 从机地址
    :param value: 减速度值
    :return: True--设置成功，False--设置失败
    """
    _check_handle_valid()

    handle_obj = RTU_HANDLE_MAP.get(handle1)

    try:
        conv_value = handle_obj.decimal_to_16bit(value)
        # print(f"减速度值转换：{value} → {conv_value}（0x{conv_value:04X}）")

    except Exception as e:
        err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_DATA_CONVERT)} - {str(e)}"
        print(f"错误：{err_msg}")
        raise ValueError(err_msg) from e

    try:
        ret_code = write_single_register(handle1, station_addr, DECELERATION_REG_ADDR, conv_value, 0, 1)

        if ret_code == ModbusError.MODBUS_SUCCESS:
            print(f"成功：电机减速度设置完成（从站{station_addr}，地址0x{DECELERATION_REG_ADDR:04X}，值{conv_value}）")
            return True

        else:
            err_desc = ModbusError.get_error_desc(
                ret_code) if ret_code in ModbusError.__members__.values() else f"未知错误码{ret_code}"
            err_msg = f"减速度设置失败，{err_desc}"
            print(f"失败：{err_msg}")
            raise OSError(err_msg, ret_code)
    except OSError:
        raise
    except Exception as e:
        err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_REG_WRITE)} - {str(e)}"
        print(f"异常：{err_msg}")
        raise Exception(err_msg) from e


def set_motor_mode(station_addr: int, mode: int) -> bool:
    """
    设置电机模式
    :param station_addr: 从机地址
    :param mode: 0--速度模式   1--绝对位置模式    2--相对位置模式
    :return: bool 设置是否成功
    """
    _check_handle_valid()

    try:
        if mode == 0:
            write_cmd = SPEED_CMD
            mode_desc = "速度模式"
        elif mode == 1:
            write_cmd = ABSOLUTE_POSITION_CMD
            mode_desc = "绝对位置模式"
        elif mode == 2:
            write_cmd = RELATIVE_POSITION_CMD
            mode_desc = "相对位置模式"

        ret_code = write_single_register(handle1, station_addr, MODE_REG_ADDR, write_cmd, resp_offset=0, use_crc=1)

        if ret_code == ModbusError.MODBUS_SUCCESS:
            print(f"成功：电机模式设置完成（从站{station_addr}，地址0x{MODE_REG_ADDR:04X}，模式：{mode_desc}）")
            return True
        else:
            err_desc = ModbusError.get_error_desc(
                ret_code) if ret_code in ModbusError.__members__.values() else f"未知错误码{ret_code}"
            err_msg = f"电机{mode_desc}设置失败，{err_desc}"
            print(f"失败：{err_msg}")
            raise OSError(err_msg, ret_code)

    except OSError:
        # 主动抛出的OSError直接上抛，不做额外处理
        raise
    except Exception as e:
        err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_MOTOR_MODE)} - {str(e)}"
        print(f"异常：{err_msg}")
        raise Exception(err_msg) from e


def get_motor_speed(station_addr: int) -> int:
    """
    获取电机速度
    :param station_addr: 从站地址
    :return: 返回电机速度值
    """
    _check_handle_valid()

    handle_obj = RTU_HANDLE_MAP.get(handle1)

    try:
        reg_values = read_holding_register(handle1, station_addr, SPEED_REG_ADDR, 1, 0, [], 1)

        if not reg_values or len(reg_values) != 1:
            err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_DATA_FORMAT)}，数据:{reg_values}"
            print(f"错误：{err_msg}")
            raise OSError(err_msg)

        unsigned_speed = reg_values[0]
        signed_speed = handle_obj.uint16_to_int16(unsigned_speed)

        print(f"成功：获取电机速度（从站{station_addr})-->无符号值:{unsigned_speed}-->有符号值:{signed_speed}")
        return signed_speed

    except OSError:
        raise

    except Exception as e:
        err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_REG_READ)} - {str(e)}"
        print(f"错误：{err_msg}")
        raise Exception(err_msg) from e


def get_motor_acceleration(station_addr: int) -> int:
    """
    获取电机加速度
    :param station_addr: 从站地址
    :return: 电机加速度值
    """
    _check_handle_valid()

    handle_obj = RTU_HANDLE_MAP.get(handle1)

    try:
        reg_values = read_holding_register(handle1, station_addr, ACCELERATION_REG_ADDR, 1, 0, [], 1)

        if not reg_values or len(reg_values) != 1:
            err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_DATA_FORMAT)}，数据:{reg_values}"
            print(f"错误：{err_msg}")
            raise OSError(err_msg)

        unsigned_speed = reg_values[0]
        signed_speed = handle_obj.uint16_to_int16(unsigned_speed)

        print(f"成功：获取电机加速度（从站{station_addr})-->无符号值:{unsigned_speed}-->有符号值:{signed_speed}")
        return signed_speed

    except OSError:
        raise

    except Exception as e:
        err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_REG_READ)} - {str(e)}"
        print(f"错误：{err_msg}")
        raise Exception(err_msg) from e


def get_motor_deceleration(station_addr: int) -> int:
    """
    获取电机减速度
    :param station_addr: 从站地址
    :return: i电机减速度值
    """
    _check_handle_valid()

    handle_obj = RTU_HANDLE_MAP.get(handle1)

    try:
        reg_values = read_holding_register(handle1, station_addr, DECELERATION_REG_ADDR, 1, 0, [], 1)

        if not reg_values or len(reg_values) != 1:
            err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_DATA_FORMAT)}，数据:{reg_values}"
            print(f"错误：{err_msg}")
            raise OSError(err_msg)

        unsigned_speed = reg_values[0]
        signed_speed = handle_obj.uint16_to_int16(unsigned_speed)

        print(f"成功：获取电机减速度（从站{station_addr})-->无符号值:{unsigned_speed}-->有符号值:{signed_speed}")
        return signed_speed

    except OSError:
        raise
    except Exception as e:
        err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_REG_READ)} - {str(e)}"
        print(f"错误：{err_msg}")
        raise Exception(err_msg) from e


def move_motor(station_addr: int, value: bool) -> bool:
    """
    启停电机
    :param station_addr: 从站地址
    :param value: True--启动电机，False--停止电机
    :return: 是否设置成功
    """
    _check_handle_valid()

    try:
        if value:
            write_cmd = START_CMD
            op = "启动"
        else:
            write_cmd = STOP_CMD
            op = "停止"

        ret_code = write_single_register(handle1, station_addr, START_OR_STOP_REG_ADDR, write_cmd, 0,1)

        if ret_code == ModbusError.MODBUS_SUCCESS:
            print(f"成功：电机{op}指令已发送（从站{station_addr}）")
            return True
        else:
            err_desc = ModbusError.get_error_desc(
                ret_code) if ret_code in ModbusError.__members__.values() else f"未知错误码{ret_code}"
            err_msg = f"电机{op}指令发送失败，{err_desc}"
            print(f"失败：{err_msg}")
            raise OSError(err_msg, ret_code)

    except OSError:
        raise
    except Exception as e:
        op = "启动" if value else "停止"
        err_msg = f"异常：电机{op}指令执行失败 - {str(e)}"
        print(f"异常:{err_msg}")
        raise Exception(err_msg)


def sync_motor_move(value: bool):
    """
    同步传送带电机
    :param value: 启停传送点电机 True--同步启动传送带电机  False--同步停止传送带电机
    """
    op_desc = "启动" if value else "停止"

    if not isinstance(value, bool):
        err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_PARAM_TYPE)}（同步{op_desc}）：参数必须是布尔值，当前值：{value}（类型：{type(value)}）"
        print(f"错误：{err_msg}")
        raise TypeError(err_msg)

    try:
        print(f"开始同步{op_desc}传送带电机（从站1、2）...")
        move_motor(1, value)
        move_motor(2, value)
        print(f"成功：所有传送带电机已同步{op_desc}完成")

    except Exception as e:
        err_msg = f"{ModbusError.get_error_desc(ModbusError.MODBUS_ERR_MOTOR_SYNC)}（{op_desc}）：{str(e)}"
        print(f"错误：{err_msg}")
        # 抛RuntimeError，保留原始异常链，便于上层定位具体失败电机
        raise RuntimeError(err_msg) from e


def read_motor_value(station_addr: int) -> int:
    """
    读取电机位置值
    :param station_addr: 从站地址
    :return: 电机位置值
    """
    _check_handle_valid()

    handle_obj = RTU_HANDLE_MAP.get(handle1)

    try:
        reg_values = read_holding_register(handle1, station_addr, ENCODER_H_REG_ADDR,
                                           reg_count=2, resp_offset=0, out_buffer=[], use_crc=1)
        if not reg_values or len(reg_values) != 2:
            err_msg = f"电机位置值读取失败，数据长度错误：{reg_values}"
            print(f"错误：{err_msg}")
            raise OSError(err_msg)

        # 组合高16位和低16位
        encoder_h = reg_values[0]  # 高16位
        encoder_l = reg_values[1]  # 低16位 
        encoder_value = (encoder_h << 16) | encoder_l
        # 转换为有符号数
        signed_encoder_value = handle_obj.uint32_to_int32(encoder_value)

        print(f"成功：读取电机位置值（从站{station_addr})-->高位:{encoder_h} 低位:{encoder_l} 总值:{signed_encoder_value}")
        return signed_encoder_value

    except OSError:
        raise
    except Exception as e:
        err_msg = f"电机位置值读取异常 - {str(e)}"
        print(f"异常：{err_msg}")   
        raise Exception(err_msg) from e


def read_motor_value_command(station_addr: int) -> int:
    """
    读取电机位置值--指令单位
    :param station_addr: 从站地址
    :return: 电机位置值
    """
    _check_handle_valid()

    handle_obj = RTU_HANDLE_MAP.get(handle1)

    try:
        reg_values = read_holding_register(handle1, station_addr, COMMAND_H_REG_ADDR,
                                           reg_count=2, resp_offset=0, out_buffer=[], use_crc=1)
        if not reg_values or len(reg_values) != 2:
            err_msg = f"电机位置值读取失败，数据长度错误：{reg_values}"
            print(f"错误：{err_msg}")
            raise OSError(err_msg)

        # 组合高16位和低16位
        command_value_h = reg_values[0]  # 高16位
        command_value_l = reg_values[1]  # 低16位   
        command_value = (command_value_h << 16) | command_value_l
        # 转换为有符号数
        signed_command_value = handle_obj.uint32_to_int32(command_value)

        print(f"成功：读取电机位置值（从站{station_addr})-->高位:{command_value_h} 低位:{command_value_l} 总值:{signed_command_value}")
        return signed_command_value 

    except OSError:
        raise   
    except Exception as e:
        err_msg = f"电机位置值读取异常 - {str(e)}"
        print(f"异常：{err_msg}")
        raise Exception(err_msg) from e

def read_position_error(station_addr: int) -> int:
    """
    读取电机位置误差--编码器单位
    :param station_addr: 从站地址
    :return: 电机位置误差
    """
    _check_handle_valid()

    handle_obj = RTU_HANDLE_MAP.get(handle1)

    try:
        reg_values = read_holding_register(handle1, station_addr, POSITION_ERROR_H_REG_ADDR,
                                           reg_count=2, resp_offset=0, out_buffer=[], use_crc=1)
        if not reg_values or len(reg_values) != 2:
            err_msg = f"电机位置误差读取失败，数据长度错误：{reg_values}"
            print(f"错误：{err_msg}")
            raise OSError(err_msg)

        # 组合高16位和低16位
        position_error_h = reg_values[0]  # 高16位
        position_error_l = reg_values[1]  # 低16位   
        position_error = (position_error_h << 16) | position_error_l
        # 转换为有符号数
        signed_position_error = handle_obj.uint32_to_int32(position_error)

        print(f"成功：读取电机位置误差（从站{station_addr})-->高位:{position_error_h} 低位:{position_error_l} 总值:{signed_position_error}")
        return signed_position_error 

    except OSError:
        raise   
    except Exception as e:
        err_msg = f"电机位置误差读取异常 - {str(e)}"
        print(f"异常：{err_msg}")
        raise Exception(err_msg) from e

# ------------对外测试接口------------
def test_speed_mode():
    # 配置传送带电机参数    只需配置一次
    # 速度模式
    set_motor_mode(station_addr=1, mode=0)    # 配置成速度模式
    set_motor_speed(station_addr=1, speed_value=-30)
    set_motor_acceleration(station_addr=1, acceleration=50)   
    set_motor_deceleration(station_addr=1, deceleration=50)

    set_motor_mode(station_addr=2, mode=0)  # 配置成速度模式
    set_motor_speed(station_addr=2, speed_value=-30)
    set_motor_acceleration(station_addr=2, acceleration=50)
    set_motor_deceleration(station_addr=2, deceleration=50) 

    sync_motor_move(True)

    time.sleep(2)

    sync_motor_move(False)
    time.sleep(0.5)

    while True: 
        time.sleep(1)

def test_absolute_position_mode():
    # 绝对位置模式
    set_motor_mode(station_addr=1, mode=1)                        # 配置成绝对位置模式
    set_motor_position(station_addr=1, position_value=0)          # 每圈10000脉冲，跑两圈
    set_motor_speed(station_addr=1, speed_value=30)
    set_motor_acceleration(station_addr=1, value=50)   
    set_motor_deceleration(station_addr=1, value=50)

    set_motor_mode(station_addr=2, mode=1)  # 配置成绝对位置模式
    set_motor_position(station_addr=2, position_value=0)
    set_motor_speed(station_addr=2, speed_value=30)
    set_motor_acceleration(station_addr=2, value=50)
    set_motor_deceleration(station_addr=2, value=50) 

    # 运行前查看电机位置初始值
    print("\n=== 运行前电机位置初始值 ===")
    motor1_initial = read_motor_value(station_addr=1)
    motor2_initial = read_motor_value(station_addr=2)
    print(f"电机1初始电机位置值: {motor1_initial}")
    print(f"电机2初始电机位置值: {motor2_initial}")

    sync_motor_move(True)

    # 等待电机完成运动（根据实际情况调整等待时间）
    time.sleep(5)

    # 读取编码器值检查是否丢步
    print("\n=== 检查电机是否丢步 ===")
    target_position = 0  # 目标位置值

    # 检查电机1
    motor1 = read_motor_value(station_addr=1)
    error1 = abs(motor1 - target_position)
    print(f"电机1 - 目标位置: {target_position}, 实际位置: {motor1}, 误差: {error1}")

    # 检查电机2
    encoder2 = read_motor_value(station_addr=2)
    error2 = abs(encoder2 - target_position)
    print(f"电机2 - 目标位置: {target_position}, 实际位置: {encoder2}, 误差: {error2}")

    encoder_unit3 = read_motor_value_command(station_addr=1)
    encoder_unit4 = read_motor_value_command(station_addr=2)
    print(f"电机1运动后电机的指令值: {encoder_unit3}")
    print(f"电机2运动后电机的指令值: {encoder_unit4}")

    # 读取电机位置误差
    position_error1 = read_position_error(station_addr=1)
    position_error2 = read_position_error(station_addr=2)
    print(f"电机1位置误差: {position_error1}")
    print(f"电机2位置误差: {position_error2}")
    
    while True: 
        time.sleep(1)

def test_relative_position_mode():
    """
    相对位置模式测试（已修复CRC错误、数据符号、通信延时问题）
    功能：每3秒相对移动6000脉冲，实时监控编码器误差，误差>100自动补偿
    """
    try:
        # ===================== 1. 基础配置 =====================
        station_list = [1, 2]
        base_pulse = 6000
        interval = 3
        error_threshold = 100
        move_wait_time = 1.5

        # 补偿值
        compensate_map = {1: 0, 2: 0}

        print("===== 相对位置模式 + 误差监控 + 自动补偿（稳定版）=====")
        print(f"基础移动：{base_pulse} 脉冲 | 误差阈值：>{error_threshold}")

        # 初始化电机
        for station in station_list:
            set_motor_mode(station, 2)
            set_motor_speed(station, 30)
            set_motor_acceleration(station, 50)
            set_motor_deceleration(station, 50)
            time.sleep(0.1)
            print(f"✅ 电机{station} 配置完成")

        print("\n===== 开始循环运动 =====")
        count = 0

        while True:
            count += 1
            print(f"\n==================== 第{count}次运动 ====================")

            # 写入位置（带重试 + 间隔，解决CRC）
            for station in station_list:
                compensate = compensate_map[station]
                final_pulse = base_pulse - compensate

                # 关键修复：相对位置必须用 有符号32位
                final_pulse = int(final_pulse)

                retry = 3
                while retry > 0:
                    try:
                        set_motor_position(station, final_pulse)
                        time.sleep(0.05)
                        break
                    except:
                        retry -= 1
                        time.sleep(0.1)
                        if retry == 0:
                            print(f"❌ 电机{station} 写入位置失败")

                print(f"电机{station}：移动 {final_pulse} 脉冲")

            # 启动
            sync_motor_move(True)
            time.sleep(move_wait_time)

            # 停止
            sync_motor_move(False)
            time.sleep(0.2)

            # 读取误差
            print("\n---------------- 误差监控 ----------------")
            for station in station_list:
                try:
                    real_pos = read_motor_value(station)
                    pos_error = read_position_error(station)

                    if abs(pos_error) > error_threshold:
                        new_comp = pos_error - error_threshold
                        compensate_map[station] = new_comp
                        status = f"⚠️  补偿：{new_comp}"
                    else:
                        compensate_map[station] = 0
                        status = "✅ 正常"

                    print(f"电机{station} | 实际：{real_pos} | 误差：{pos_error} | {status}")
                except:
                    print(f"⚠️  电机{station} 读取失败")

            print(f"\n⏳ {interval}秒后继续...")
            time.sleep(interval)

    except KeyboardInterrupt:
        print("\n🛑 手动停止")
        sync_motor_move(False)
    except Exception as e:
        print(f"\n❌ 异常：{e}")
        sync_motor_move(False)

def test_cycle_relative_position_mode():
    """
    相对位置模式 —— 前20次每次+5，之后固定使用6007脉冲
    稳定版：防CRC报错 + 自动重试
    """
    try:
        # ===================== 配置 =====================
        station_list = [1, 2]
        base_pulse = 6007          # 初始脉冲
        move_wait_time = 3          # 运动时间
        send_interval = 2           # 间隔2秒
        speed = 30
        acc = 50
        dec = 50

        max_increase_times = 50     # 前20次递增
        increase_step = 2           # 每次加5
        current_count = 0           # 运行次数计数

        print("===== 前20次每次+5，20次后恢复6007脉冲 =====")

        # 初始化电机
        for station in station_list:
            set_motor_mode(station, 2)
            time.sleep(0.1)
            set_motor_speed(station, speed)
            time.sleep(0.1)
            set_motor_acceleration(station, acc)
            time.sleep(0.1)
            set_motor_deceleration(station, dec)
            time.sleep(0.1)
            print(f"✅ 电机{station} 初始化完成")

        # ===================== 主循环 =====================
        while True:
            current_count += 1
            print(f"\n==================== 第 {current_count} 次运动 ====================")

            # ============== 核心逻辑 ==============
            if current_count <= max_increase_times:
                # 前20次：每次 +5
                send_pulse = base_pulse + current_count * increase_step
                print(f"📈 前20次递增 → 本次发送：{send_pulse}")
            else:
                # 20次以后：固定 6007
                send_pulse = 6007
                print(f"✅ 已超过20次 → 固定发送：{send_pulse}")

            # ========== 写入位置（带重试）==========
            for station in station_list:
                retry = 5
                while retry > 0:
                    try:
                        set_motor_position(station, send_pulse)
                        time.sleep(0.1)
                        break
                    except Exception as e:
                        retry -= 1
                        print(f"⚠️  电机{station} 写入失败，重试 {retry} 次")
                        time.sleep(0.2)

            # ========== 启动电机 ==========
            sync_motor_move(True)

            # ========== 运动中实时监控误差 ==========
            print("\n------------ 运动中实时误差 ------------")
            start_time = time.time()
            while time.time() - start_time < move_wait_time:
                for station in station_list:
                    try:
                        err = read_position_error(station)
                        print(f"电机{station} 实时误差 → {err}")
                    except:
                        pass
                time.sleep(0.1)

            # ========== 停止 + 等待 ==========
            sync_motor_move(False)
            time.sleep(0.3)
            print(f"\n✅ 运动完成，等待 {send_interval} 秒后继续...")
            time.sleep(send_interval)

    except KeyboardInterrupt:
        print("\n🛑 手动停止")
        try:
            sync_motor_move(False)
        except:
            pass
    except Exception as e:
        print(f"\n❌ 异常：{e}")
        try:
            sync_motor_move(False)
        except:
            pass

def test_cycle_relative_position_mode_test():
    """
    相对位置模式 —— 电机1每次+2，电机2每次+4
    稳定版：防CRC报错 + 自动重试
    """
    try:
        # ===================== 配置 =====================
        station_list = [1, 2]
        base_pulse = 6007          # 初始基准脉冲
        move_wait_time = 3         # 运动时间
        send_interval = 2          # 间隔2秒
        speed = 30
        acc = 50
        dec = 50

        current_count = 0          # 运行次数计数

        print("===== 电机1每次+2，电机2每次+4 循环运行 =====")

        # 初始化电机
        for station in station_list:
            set_motor_mode(station, 2)
            time.sleep(0.1)
            set_motor_speed(station, speed)
            time.sleep(0.1)
            set_motor_acceleration(station, acc)
            time.sleep(0.1)
            set_motor_deceleration(station, dec)
            time.sleep(0.1)
            print(f"✅ 电机{station} 初始化完成")

        # ===================== 主循环 =====================
        while True:
            current_count += 1
            print(f"\n==================== 第 {current_count} 次运动 ====================")

            # ============== 核心：两台电机不同增量 ==============
            pulse_1 = base_pulse + current_count * 10  # 电机1：每次 +2      靠近出料口的电机
            pulse_2 = base_pulse + current_count * 5   # 电机2：每次 +4      远离出料口的电机

            print(f"📌 电机1本次脉冲：{pulse_1}")
            print(f"📌 电机2本次脉冲：{pulse_2}")

            # ========== 分别写入两台电机（带重试）==========
            # 写入电机1
            retry = 5
            while retry > 0:
                try:
                    set_motor_position(1, pulse_1)
                    time.sleep(0.1)
                    break
                except Exception as e:
                    retry -= 1
                    print(f"⚠️  电机1写入失败，重试 {retry} 次")
                    time.sleep(0.2)

            # 写入电机2
            retry = 5
            while retry > 0:
                try:
                    set_motor_position(2, pulse_2)
                    time.sleep(0.1)
                    break
                except Exception as e:
                    retry -= 1
                    print(f"⚠️  电机2写入失败，重试 {retry} 次")
                    time.sleep(0.2)

            # ========== 启动电机 ==========
            sync_motor_move(True)

            # ========== 运动中实时监控误差 ==========
            print("\n------------ 运动中实时误差 ------------")
            start_time = time.time()
            while time.time() - start_time < move_wait_time:
                for station in station_list:
                    try:
                        err = read_position_error(station)
                        print(f"电机{station} 实时误差 → {err}")
                    except:
                        pass
                time.sleep(0.1)

            time.sleep(1)
            print(f"\n✅ 运动完成，等待 {send_interval} 秒后继续...")
            time.sleep(send_interval)

    except KeyboardInterrupt:
        print("\n🛑 手动停止")
        try:
            sync_motor_move(False)
        except:
            pass
    except Exception as e:
        print(f"\n❌ 异常：{e}")
        try:
            sync_motor_move(False)
        except:
            pass

# ------------调试接口----------
if __name__ == '__main__':
    test_cycle_relative_position_mode()