零差云控EROB电机python控制（CanOpen）

zzZ··*

已于 2024-11-12 15:23:06 修改

阅读量493

点赞数 6

分类专栏： CanOpen 文章标签： python 机器人 github 开源

于 2024-11-11 15:27:18 首次发布

本文链接：https://blog.csdn.net/freeze_/article/details/143686739

版权

CanOpen 专栏收录该内容

1 篇文章

订阅专栏

Github地址 Gitee地址

适用于零差云控ZeroErr公司旗下的EROB关节电机的python控制库，CanOpen通讯

详情见Github地址，还请点赞点个Star！

当前进度：

1.完成轮廓位置模式（PP）

2.完成周期位置同步模式（CSP）

3.完成轮廓扭矩模式（PT）

4.完善多电机PP模式控制

5.添加简单GUI界面

注意，这个电机的初始化非常坑，多电机下经常出现初始化失败或者掉使能的情况。

MotorControl_PP.py

import canopen
import time
import logging
import can
import threading

# 配置日志记录
logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)

class Motor_PP:
    def __init__(self, node_id, network, encoder_resolution=524288):
        self.node_id = node_id
        self.network = network
        self.encoder_resolution = encoder_resolution
        self.motor = self.network.add_node(self.node_id, "ZeroErr Driver_V1.5.eds")
        self.motor.load_configuration()  # 加载配置，防止覆盖自定义PDO映射
        self.initialize_node()  # 初始化节点
        self.configure_pdo()  # 配置PDO映射
        time.sleep(0.1)
        self.start_node()  # 启动节点
        self.set_immediate_effect(True)  # 设置立即生效
        self.clear_fault()  # 清除故障
        self.enable_motor()  # 电机使能
        self.monitoring = threading.Event()  # 控制监控线程的事件
        self.current_position = 0            # 实时存储电机当前位置
        self.tracking_target = False         # 是否在跟踪目标位置
        self.target_position = 0             # 目标位置
        self.monitor_thread = None           # 初始化监控线程为 None

    def angle_to_position(self, angle):
        """将角度转换为位置脉冲值"""
        position = (angle / 360) * self.encoder_resolution
        return int(position)

    def position_to_angle(self, position):
        """将位置脉冲值转换为角度"""
        angle = (position / self.encoder_resolution) * 360
        return angle
    
    def velocity_to_pulse(self, velocity_rpm):
        """将速度转换为脉冲值"""
        velocity_pulse_per_sec = (velocity_rpm / 60) * self.encoder_resolution
        return int(velocity_pulse_per_sec)

    def acceleration_to_pulse(self, acceleration_rpm2):
        """将加速度转换为脉冲值"""
        acceleration_pulse_per_sec2 = (acceleration_rpm2 / 60) * self.encoder_resolution
        return int(acceleration_pulse_per_sec2)

    def clear_fault(self):
        """清除电机的故障状���"""
        logger.info(f"电机 {self.node_id}: 清除故障")
        self.set_controlword(0x0080)  # 发送复位故障命令
        time.sleep(0.1)
        self.set_controlword(0x0006)  # 进入准备就绪状态
        time.sleep(0.1)

    def initialize_node(self):
        """初始化节点"""
        # 停止远程节点
        message = can.Message(
            arbitration_id=0x000,      # NMT 指令的 COB-ID 是 0x000
            data=[0x02, self.node_id], # 0x02 是 Stop remote node，第二字节是节点 ID
            is_extended_id=False
        )
        self.network.bus.send(message)
        logger.info(f"电机 {self.node_id}:停止远程节点指令发送完成")

        # 复位节点
        message = can.Message(
            arbitration_id=0x000,      # NMT 指令的 COB-ID 是 0x000
            data=[0x82, self.node_id], # 0x82 是 Reset communication，第二字节是节点 ID
            is_extended_id=False
        )
        self.network.bus.send(message)
        logger.info(f"电机 {self.node_id}:复位节点指令发送完成")

        # 设置为轮廓位置模式
        self.motor.sdo[0x6060].raw = 0x01  # 发送 2F 60 60 00 01 00 00 00
        logger.info(f"电机 {self.node_id}:轮廓位置模式设置完成")

        # 核对运行模式为 PP 模式
        if self.motor.sdo[0x6061].raw == 0x01:  # 发送 40 61 60 00 00 00 00 00
            logger.info(f"电机 {self.node_id}:运行模式为 PP 模式")
        else:
            logger.warning(f"电机 {self.node_id}:运行模式非 PP 模式，请检查配置")

        # 设置轮廓速度为 20°/s
        self.motor.sdo[0x6081].raw = self.velocity_to_pulse(5)
        logger.info(f"电机 {self.node_id}:轮廓速度设置为 10°/s")

        # 设置轮廓加速度为 20°/s²
        self.motor.sdo[0x6083].raw = self.acceleration_to_pulse(5)
        logger.info(f"电机 {self.node_id}:轮廓加速度设置为 10°/s²")

        # 设置轮廓减速度为 20°/s²
        self.motor.sdo[0x6084].raw = self.acceleration_to_pulse(5)
        logger.info(f"电机 {self.node_id}:轮廓减速度设置为 10°/s²")

        # 关闭同步发生器
        self.motor.sdo[0x1006].raw = 0  # 发送 23 05 10 00 00 00 00 00
        logger.info(f"电机 {self.node_id}:同步发生器已关闭")

        # 通信周期设置为 1000us
        self.motor.sdo[0x1006].raw = 1000  # 发送 23 06 10 00 E8 03 00 00
        logger.info(f"电机 {self.node_id}:通信周期设置为 1000us")

    def set_profile_velocity(self, velocity_rpm):
        """设置轮廓速度"""
        self.motor.sdo[0x6081].raw = self.velocity_to_pulse(velocity_rpm)
        logger.info(f"电机 {self.node_id}: 轮廓速度设置为 {velocity_rpm}°/s")

    def set_profile_acceleration(self, acceleration_rpm2):
        """设置轮廓加速度"""
        self.motor.sdo[0x6083].raw = self.acceleration_to_pulse(acceleration_rpm2)
        logger.info(f"电机 {self.node_id}: 轮廓加速度设置为 {acceleration_rpm2}°/s²")

    def set_profile_deceleration(self, deceleration_rpm2):
        """设置轮廓减速度"""
        self.motor.sdo[0x6084].raw = self.acceleration_to_pulse(deceleration_rpm2)
        logger.info(f"电机 {self.node_id}: 轮廓减速度设置为 {deceleration_rpm2}°/s²")

    def set_profile_parameters(self, velocity_rpm, acceleration_rpm2, deceleration_rpm2):
        """一起设置轮廓速度、加速度和减速度"""
        self.set_profile_velocity(velocity_rpm)
        self.set_profile_acceleration(acceleration_rpm2)
        self.set_profile_deceleration(deceleration_rpm2)
        logger.info(f"电机 {self.node_id}: 轮廓参数设置完成 - 速度: {velocity_rpm}°/s, 加速度: {acceleration_rpm2}°/s², 减速度: {deceleration_rpm2}°/s²")

    def configure_pdo(self):
        """根据官方手册配置 TxPDO1 和 RxPDO1 的映射"""
        
        # 计算基于 node_id 的 COB-ID
        txpdo1_cob_id = 0x180 + self.node_id
        rxpdo1_cob_id = 0x200 + self.node_id
        self.motor.nmt.state = 'PRE-OPERATIONAL'
        logger.debug(f"电机 {self.node_id}:进入 PRE-OPERATIONAL 状态")

        # 配置 TxPDO1
        logger.info(f"电机 {self.node_id}:开始配置 TxPDO1")
        
        # 1. 禁用 TxPDO1
        self.motor.sdo[0x1800][1].raw = txpdo1_cob_id | 0x80000000  # COB-ID + 禁用位
        logger.debug(f"电机 {self.node_id}:关闭 TxPDO1")

        # 2. 设置传输类型
        self.motor.sdo[0x1800][2].raw = 0x01  # 异步传输
        logger.debug(f"电机 {self.node_id}:设置 TxPDO1 传输类型为异步传输 (0x01)")

        # 3. 清除 TxPDO1 映射
        self.motor.sdo[0x1A00][0].raw = 0x00
        logger.debug(f"电机 {self.node_id}:清除 TxPDO1 原有映射")

        # 4. 设置映射对象：状态字 (Status Word)
        self.motor.sdo[0x1A00][1].raw = 0x60410010
        logger.debug(f"电机 {self.node_id}:映射状态字 (Status Word) 到 TxPDO1")

        # 5. 设置映射对象：实际位置 (Actual Position)
        self.motor.sdo[0x1A00][2].raw = 0x60640020
        logger.debug(f"电机 {self.node_id}:映射实际位置 (Actual Position) 到 TxPDO1")

        # 6. 设置 TxPDO1 映射对象数量为 2
        self.motor.sdo[0x1A00][0].raw = 0x02
        logger.debug(f"电机 {self.node_id}:TxPDO1 映射对象个数设置为 2")

        # 7. 设置传输类型并启用 TxPDO1
        self.motor.sdo[0x1800][2].raw = 0xFF  # 异步传输类型
        self.motor.sdo[0x1800][1].raw = txpdo1_cob_id  # 去掉禁用位，设置 COB-ID
        logger.debug(f"电机 {self.node_id}:开启 TxPDO1 并设置传输类型为异步 (0xFF)")

        # 配置 RxPDO1
        logger.info(f"电机 {self.node_id}:开始配置 RxPDO1")

        # 1. 禁用 RxPDO1
        self.motor.sdo[0x1400][1].raw = rxpdo1_cob_id | 0x80000000  # COB-ID + 禁用位
        logger.debug(f"电机 {self.node_id}:关闭 RxPDO1")

        # 2. 设置传输类型
        self.motor.sdo[0x1400][2].raw = 0x01  # 异步传输
        logger.debug(f"电机 {self.node_id}:设置 RxPDO1 传输类型为异步传输 (0x01)")

        # 3. 清除 RxPDO1 映射
        self.motor.sdo[0x1600][0].raw = 0x00
        logger.debug(f"电机 {self.node_id}:清除 RxPDO1 原有映射")

        # 4. 设置映射对象：控制字 (Control Word)
        self.motor.sdo[0x1600][1].raw = 0x60400010
        logger.debug(f"电机 {self.node_id}:映射控制字 (Control Word) 到 RxPDO1")

        # 5. 设置映射对象：目标位置 (Target Position)
        self.motor.sdo[0x1600][2].raw = 0x607A0020
        logger.debug(f"电机 {self.node_id}:映射目标位置 (Target Position) 到 RxPDO1")

        # 6. 设置 RxPDO1 映射对象数量为 2
        self.motor.sdo[0x1600][0].raw = 0x02
        logger.debug(f"电机 {self.node_id}:RxPDO1 映射对象个数设置为 2")

        # 7. 设置传输类型并启用 RxPDO1
        self.motor.sdo[0x1400][2].raw = 0xFF  # 异步传输类型
        self.motor.sdo[0x1400][1].raw = rxpdo1_cob_id  # 去掉禁用位，设置 COB-ID
        logger.debug(f"电机 {self.node_id}:开启 RxPDO1 并设置传输类型为异步 (0xFF)")

        logger.info(f"电机 {self.node_id}:PDO 配置完成")
    
    def start_node(self):
        """启动节点"""
        self.network.nmt.send_command(0x01)  # NMT启动节点
        logger.info(f"电机 {self.node_id}:NMT启动远程节点完成")
        # 获取实际位置
        actual_position = self.motor.sdo[0x6064].raw  # 0x6064 是实际位置的对象字典索引
        actual_position = self.position_to_angle(actual_position)
        logger.info(f"实际位置：{round(actual_position, 2)}°")
        # 发送同步帧
        self.send_sync_frame()

    def set_immediate_effect(self, immediate):
        """设置立即生效或非立即生效"""
        controlword = self.motor.sdo[0x6040].raw
        if immediate:
            controlword |= (1 << 5)  # 设置Bit 5为1（立即生效）
            logger.info(f"电机 {self.node_id}:设置为立即生效")
        else:
            controlword &= ~(1 << 5)  # 设置Bit 5为0（非立即生效）
            logger.info(f"电机 {self.node_id}:设置为非立即生效")
        self.motor.sdo[0x6040].raw = controlword
        logger.debug(f"电机 {self.node_id}:控制字已更新为: {hex(controlword)}")

    def send_sync_frame(self):
        """发送同步帧"""
        sync_message = can.Message(arbitration_id=0x080, data=[], is_extended_id=False)
        self.network.bus.send(sync_message)
        logger.debug(f"电机 {self.node_id}:同步帧发送完成")

    def enable_motor(self):
        """电机使能过程，使用PDO进行控制"""
        self.set_controlword(0x06)  # Shutdown (关闭)
        self.send_sync_frame()
        time.sleep(0.1)
        self.set_controlword(0x07)  # Switch on (准备使能)
        self.send_sync_frame()
        time.sleep(0.1)
        self.set_controlword(0x0F)  # Enable operation (使能)
        self.send_sync_frame()
        time.sleep(0.1)
        logger.info(f"电机 {self.node_id}:电机使能完成")

    def check_target_reached(self):
        """监控目标位置是否到达"""
        #statusword = self.motor.pdo.tx[1]['Statusword'].raw
        statusword = self.motor.sdo[0x6041].raw  # 读取 Statusword (0x6041)
        return (statusword & 0x0400) != 0  # 检查Statusword的第10位

    def monitor_target_reached(self, timeout=10):
        """监控电机是否到达目标位置"""
        start_time = time.time()
        while self.monitoring.is_set():
            if self.check_target_reached():
                actual_position = self.motor.sdo[0x6064].raw
                logger.info(f"电机 {self.node_id}: 已到达目标位置, 当前实际位置: {actual_position}")
                self.monitoring.clear()  # 停止监控
                break
            if time.time() - start_time > timeout:
                logger.warning(f"电机 {self.node_id}: 未能在 {timeout} 秒内到达目标位置")
                self.monitoring.clear()
                break
            logger.info(f"电机 {self.node_id}: 目标未到达。当前位置: {self.get_actual_position()}")
            time.sleep(0.1)

    def get_actual_position(self):
        """获取电机的当前实际位置，通过PDO读取"""
        actual_position = self.motor.pdo.tx[1]['Position actual value'].raw
        logger.debug(f"电机 {self.node_id}: 当前实际位置为 {actual_position} plus")
        return actual_position
    
    def set_controlword(self, controlword):
        """设置控制字到 0x6040"""
        self.motor.pdo.rx[1]['Controlword'].raw = controlword
        self.motor.pdo.rx[1].transmit()
        logger.debug(f"电机 {self.node_id}:通过 PDO 发送控制字: {hex(controlword)}")

    def go_to_position(self, angle):
        """移动电机到指定角度"""
        position = self.angle_to_position(angle)
        self.target_position = position
        self.tracking_target = True
        self.motor.pdo.rx[1]['Target Position'].raw = position  # 设置目标位置
        self.send_sync_frame()
        self.set_controlword(0x0F)  # Enable operation (使能)
        self.send_sync_frame()
        self.set_controlword(0x3F)  # Command triggered (命令触发)
        self.send_sync_frame()

    def get_current_angle(self):
        """获取当前角度"""
        return self.position_to_angle(self.current_position)

    def start_angle_monitoring(self):
        """启动角度监控线程"""
        if self.monitor_thread is None or not self.monitor_thread.is_alive():
            self.monitoring.clear()  # 确保线程停止标志未被设置
            self.monitor_thread = threading.Thread(target=self.position_monitor)
            self.monitor_thread.daemon = True
            self.monitor_thread.start()
            logger.info(f"电机 {self.node_id}: 角度监控线程已启动")

    def stop_angle_monitoring(self):
        """停止角度监控线程"""
        if self.monitor_thread and self.monitor_thread.is_alive():
            self.monitoring.set()      # 设置事件，通知线程停止
            self.monitor_thread.join()  # 等待线程结束
            self.monitor_thread = None
            logger.info(f"电机 {self.node_id}: 角度监控线程已停止")

    def position_monitor(self):
        """持续监控电机位置的线程方法"""
        while not self.monitoring.is_set():
            # 获取实际位置并更新
            position = self.get_actual_position()
            self.current_position = position

            if self.tracking_target:
                angle = self.position_to_angle(position)
                logger.info(f"电机 {self.node_id}: 当前角度: {round(angle, 2)}°")
                if self.check_target_reached():
                    logger.info(f"电机 {self.node_id}: 已到达目标位置 {round(self.position_to_angle(self.target_position), 2)}°")
                    self.tracking_target = False
            time.sleep(0.1)
        logger.info(f"电机 {self.node_id}: 角度监控线程已退出")

def main():
    """主程序，实例化多个电机并控制它们"""
    network = canopen.Network()
    network.connect(bustype='pcan', channel='PCAN_USBBUS1', bitrate=1000000)
    stop_event = threading.Event()


    try:
        motor = Motor_PP(0x04, network)

        # 启动角度监控线程
        motor.start_angle_monitoring()

        while True:
            angle_input = input("请输入角度值 (按回车执行, 输入 'q' 退出, 输入 'p' 获取当前位置): ")
            if angle_input.lower() == 'q':
                break
            elif angle_input.lower() == 'p':
                current_angle = motor.get_current_angle()
                print(f"电机 {motor.node_id}: 当前位置: {round(current_angle, 2)}°")
            else:
                try:
                    angle = float(angle_input)
                    motor.go_to_position(angle)
                except ValueError:
                    logger.error("请输入有效的数字")
                except Exception as e:
                    logger.error(f"发生错误: {e}")

    finally:
        # 停止角度监控线程
        motor.stop_angle_monitoring()

        stop_event.set()        # 停止网络处理线程
        network.disconnect()
        logger.info("已断开与CAN网络的连接")

if __name__ == "__main__":
    main()

MotorControl_CSP.py

import canopen
import time
import logging
import can
import threading
import numpy as np
from scipy.interpolate import make_interp_spline

# 配置日志记录
logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)

class Motor_CSP:
    def __init__(self, node_id, network, encoder_resolution=524288):
        self.node_id = node_id
        self.network = network
        self.encoder_resolution = encoder_resolution
        self.motor = self.network.add_node(self.node_id, "ZeroErr Driver_V1.5.eds")
        self.motor.load_configuration()  # 加载配置，防止覆盖自定义PDO映射
        self.initialize_node()  # 初始化节点
        self.setup_pdo_for_csp()  # 配置PDO映射
        time.sleep(0.1)
        self.start_node()  # 启动节点
        time.sleep(0.1)
        self.clear_fault()  # 清除故障
        time.sleep(0.1)
        self.enable_motor()  # 电机使能
        time.sleep(0.1)


    def angle_to_position(self, angle):
        """将角度转换为位置脉冲值"""
        position = (angle / 360) * self.encoder_resolution
        return int(position)

    def position_to_angle(self, position):
        """将位置脉冲值转换为角度"""
        angle = (position / self.encoder_resolution) * 360
        return angle
    
    def velocity_to_pulse(self, velocity_rpm):
        """将速度转换为脉冲值"""
        velocity_pulse_per_sec = (velocity_rpm / 60) * self.encoder_resolution
        return int(velocity_pulse_per_sec)

    def acceleration_to_pulse(self, acceleration_rpm2):
        """将加速度转换为脉冲值"""
        acceleration_pulse_per_sec2 = (acceleration_rpm2 / 60) * self.encoder_resolution
        return int(acceleration_pulse_per_sec2)

    def clear_fault(self):
        """清除电机的故障状���"""
        logger.info(f"电机 {self.node_id}: 清除故障")
        self.set_controlword(0x0080)  # 发送复位故障命令
        time.sleep(0.1)
        self.set_controlword(0x0006)  # 进入准备就绪状态
        time.sleep(0.1)

    def initialize_node(self):
        """初始化节点"""
        # 停止远程节点
        message = can.Message(
            arbitration_id=0x000,      # NMT 指令的 COB-ID 是 0x000
            data=[0x02, self.node_id], # 0x02 是 Stop remote node，第二字节是节点 ID
            is_extended_id=False
        )
        self.network.bus.send(message)
        logger.info(f"电机 {self.node_id}:停止远程节点指令发送完成")

        # 复位节点
        message = can.Message(
            arbitration_id=0x000,      # NMT 指令的 COB-ID 是 0x000
            data=[0x82, self.node_id], # 0x82 是 Reset communication，第二字节是节点 ID
            is_extended_id=False
        )
        self.network.bus.send(message)
        logger.info(f"电机 {self.node_id}:复位节点指令发送完成")

        # 设置为CSP模式
        self.motor.sdo[0x6060].raw = 0x08  # 发送 2F 60 60 00 01 00 00 00
        logger.info(f"电机 {self.node_id}:周期位置同步模式设置完成")

        # 核对运行模式为 CSP 模式
        if self.motor.sdo[0x6061].raw == 0x08:  # 发送 40 61 60 00 00 00 00 00
            logger.info(f"电机 {self.node_id}:运行模式为 CSP 模式")
        else:
            logger.warning(f"电机 {self.node_id}:运行模式非 CSP 模式，请检查配置")
        time.sleep(0.1)

        # 关闭同步发生器
        self.motor.sdo[0x1006].raw = 0  # 发送 23 05 10 00 00 00 00 00
        logger.info(f"电机 {self.node_id}:同步发生器已关闭")

        # 通信周期设置为 1000us
        self.motor.sdo[0x1006].raw = 1000  # 发送 23 06 10 00 E8 03 00 00
        logger.info(f"电机 {self.node_id}:通信周期设置为 1000us")

    def setup_pdo_for_csp(self):
        """配置RxPDO和TxPDO映射"""
        self.motor.nmt.state = 'PRE-OPERATIONAL'

        # 配置RxPDO1：映射控制字和目标位置
        self.motor.sdo[0x1400][1].raw = 0x80000200 + self.node_id  # 禁用RxPDO1
        self.motor.sdo[0x1600][0].raw = 0  # 清空映射条目
        self.motor.sdo[0x1600][1].raw = 0x60400010  # 控制字（16位）
        self.motor.sdo[0x1600][2].raw = 0x607A0020  # 目标位置（32位）
        self.motor.sdo[0x1600][0].raw = 2  # 映射两个对象
        self.motor.sdo[0x1400][1].raw = 0x00000200 + self.node_id  # 启用RxPDO1
        #self.motor.sdo[0x1400][2].raw = 0x01  

        # 配置TxPDO1：映射状态字和实际位置
        self.motor.sdo[0x1800][1].raw = 0x80000180 + self.node_id  # 禁用TxPDO1
        self.motor.sdo[0x1A00][0].raw = 0  # 清空映射条目
        self.motor.sdo[0x1A00][1].raw = 0x60410010  # 状态字（16位）
        self.motor.sdo[0x1A00][2].raw = 0x60640020  # 实际位置（32位）
        self.motor.sdo[0x1A00][0].raw = 2  # 映射两个对象
        self.motor.sdo[0x1800][1].raw = 0x00000180 + self.node_id  # 启用TxPDO1
       #self.motor.sdo[0x1800][2].raw = 0x01  

        # 启用PDO通信
        self.motor.pdo.rx[1].enabled = True
        self.motor.pdo.tx[1].enabled = True

        self.motor.nmt.state = 'OPERATIONAL'  # 切换到操作模式

        logger.info(f"电机 {self.node_id} PDO映射配置完成")


    def start_node(self):
        """启动节点"""
        self.network.nmt.send_command(0x01)  # NMT启动节点
        logger.info(f"电机 {self.node_id}:NMT启动远程节点完成")
        # 获取实际位置
        actual_position = self.motor.sdo[0x6064].raw  # 0x6064 是实际位置的对象字典索引
        # 写入实际位置
        self.motor.sdo[0x607A].raw = actual_position
        actual_position = self.position_to_angle(actual_position)
        logger.info(f"实际位置：{round(actual_position, 2)}°")


    def send_sync_frame(self):
        """发送同步帧"""
        sync_message = can.Message(arbitration_id=0x080, data=[], is_extended_id=False)
        self.network.bus.send(sync_message)
        logger.debug(f"电机 {self.node_id}:同步帧发送完成")
        
    def send_controlword(self, controlword):
        """通过PDO发送控制字"""
        self.motor.pdo.rx[1]['Controlword'].raw = controlword
        self.motor.pdo.rx[1].transmit()
        self.send_sync_frame()

    def check_statusword(self, expected_value, mask, description):
        """检查状态字是否符合预期值"""
        for _ in range(50):  # 最多等待5秒钟
            statusword = self.motor.pdo.tx[1]['Statusword'].raw
            if (statusword & mask) == expected_value:
                logger.info(f"电机 {self.node_id}: 进入 {description} 状态")
                return
            time.sleep(0.1)
        raise Exception(f"电机 {self.node_id}: 未能进入 {description} 状态，当前状态字：0x{statusword:04X}")

    def enable_motor(self):
        """电机使能过程，使用PDO进行控制"""
        # 步骤：Shutdown -> Switch On -> Operation Enabled
        self.send_controlword(0x0006)  # Shutdown
        time.sleep(0.1)
        self.check_statusword(0x0021, 0x006F, "Ready to Switch On")

        self.send_controlword(0x0007)  # Switch On
        time.sleep(0.1)
        #self.check_statusword(0x0023, 0x006F, "Switched On")

        self.send_controlword(0x000F)  # Operation Enabled
        time.sleep(0.1)
        #self.check_statusword(0x0027, 0x006F, "Operation Enabled")

        
        logger.info(f"电机 {self.node_id}:电机使能完成")

    def check_target_reached(self):
        """监控目标位置是否到达"""
        #statusword = self.motor.pdo.tx[1]['Statusword'].raw
        statusword = self.motor.sdo[0x6041].raw  # 读取 Statusword (0x6041)
        return (statusword & 0x0400) != 0  # 检查Statusword的第10位


    def get_actual_position(self):
        """获取电机的当前实际位置，通过PDO读取"""
        actual_position = self.motor.pdo.tx[1]['Position actual value'].raw
        logger.debug(f"电机 {self.node_id}: 当前实际位置为 {actual_position} plus")
        return actual_position
    
    def set_controlword(self, controlword):
        """设置控制字到 0x6040"""
        self.motor.pdo.rx[1]['Controlword'].raw = controlword
        self.motor.pdo.rx[1].transmit()
        logger.debug(f"电机 {self.node_id}:通过 PDO 发送控制字: {hex(controlword)}")

    def go_to_position(self, position):
        """移动电机到指定角度"""
        self.motor.pdo.rx[1]['Target Position'].raw = position
        self.motor.pdo.rx[1].transmit()
        #self.send_sync_frame()

def main():
    network = canopen.Network()
    network.connect(bustype='pcan', channel='PCAN_USBBUS1', bitrate=1000000)

    def GO(traj, motor,interval=0.01):
        for position in traj:
            # 记录开始时间
            start_time = time.perf_counter()           
            # 更新目标位置
            motor.go_to_position(position)
            #motor2.go_to_position(position)
            motor.send_sync_frame()     
            # 计算需要等待的时间
            elapsed_time = time.perf_counter() - start_time
            sleep_time = max(0, interval - elapsed_time)          
            # 保证精确的间隔时间
            time.sleep(sleep_time)

    try:
        motor1 = Motor_CSP(0x04, network)
        time.sleep(0.5)
        #motor2 = Motor_CSP(0x03, network)
        # 设置控制点
        x_points = np.array([0, 500, 1000])  # X轴上的控制点
        y_points = np.array([0, 180, 0])  # Y轴上的控制点
        # 创建三次样条曲线对象
        spline = make_interp_spline(x_points, y_points, k=3, bc_type=([(1, 0.0)], [(1, 0.0)]))  # k=3表示三次样条
        # 生成 1000 个均匀分布的 x 值
        x_1000 = np.linspace(0, 1000, 1000)
        y_1000 = spline(x_1000)  # 计算每个 x 值对应的 y 值
        # 将 y 值存入数组
        y_values_array = y_1000
        target_positions = [motor1.angle_to_position(angle) for angle in y_values_array]

        # 设置控制点
        x_points1 = np.array([0, 500, 1000])  # X轴上的控制点
        y_points1 = np.array([45, 135, 45])  # Y轴上的控制点
        # 创建三次样条曲线对象
        spline1 = make_interp_spline(x_points1, y_points1, k=3, bc_type=([(1, 0.0)], [(1, 0.0)]))  # k=3表示三次样条
        # 生成 1000 个均匀分布的 x 值
        x_10001 = np.linspace(0, 1000, 1000)
        y_10001 = spline1(x_10001)  # 计算每个 x 值对应的 y 值
        # 将 y 值存入数组
        y_values_array1 = y_10001
        target_positions1 = [motor1.angle_to_position(angle) for angle in y_values_array1]
        interval = 0.01  # 10毫秒
        # arry1 = np.linspace(0, 45, 500)
        # target_positions_init = [motor1.angle_to_position(angle) for angle in arry1]
        time.sleep(1)
        motor1.send_sync_frame()
        actuall_position = motor1.get_actual_position()
        motor1.go_to_position(actuall_position)
        motor1.send_sync_frame()
        motor1.clear_fault()
        time.sleep(1)
        motor1.enable_motor()
        time.sleep(1)
        GO(target_positions1, motor1)

        logger.info("CSP轨迹跟随完成")
        crrent_position = motor1.get_actual_position()
        crrent_position = motor1.position_to_angle(crrent_position)
        logger.info(f"当前位置：{crrent_position}")

    finally:

        network.disconnect()
        logger.info("已断开与CAN网络的连接")


if __name__ == "__main__":
    main()