wire_controlsystem/DM/DM_Motor.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''
# @Time    : 2025/11/5 14:27
# @Author  : reenrr
# @File    : DM_Motor.py
# Description：线条厂--达妙电机控制程序
'''
from DM_CAN import *
import serial
import time

# -------------------------- 电机参数配置 --------------------------
SLAVE_ID = 0x01
MASTER_ID = 0x11
PORT = 'COM10'
BAUDRATE = 921600

class DMMotorController:
    """达妙电机控制器类"""
    def __init__(self, slave_id=None, master_id=None, port=None, baudrate=None):
        """
        初始化电机控制器
        :param slave_id: 从机ID，默认0x01
        :param master_id: 主机ID，默认0x11
        :param port: 串口端口，默认COM6
        :param baudrate: 波特率，默认921600
        """
        # 初始化参数
        self.slave_id = slave_id if slave_id is not None else SLAVE_ID
        self.master_id = master_id if master_id is not None else MASTER_ID
        self.port = port if port is not None else PORT
        self.baudrate = baudrate if baudrate is not None else BAUDRATE

        # 核心属性初始化
        self.serial_device = None    # 串口设备
        self.motor = None            # 电机实例
        self.motor_control = None    # 电机控制器实例

        # 初始化电机和串口
        self.init_motor()

    def init_motor(self):
        """初始化电机和串口"""
        try:
            # 1.初始化串口
            self.serial_device = serial.Serial(
                port=self.port,
                baudrate=self.baudrate,
                timeout=0.5
            )

            # 2.创建电机实例
            self.motor = Motor(DM_Motor_Type.DM4310, self.slave_id, self.master_id)
            # 3.创建电机控制器实例
            self.motor_control = MotorControl(self.serial_device)
            # 4.添加电机
            self.motor_control.addMotor(self.motor)

            print(f"✅ 电机初始化成功")
            print(f"  - 串口：{self.port} | 波特率：{self.baudrate}")
            print(f"  - 从机ID：{hex(self.slave_id)} | 主机ID：{hex(self.master_id)}")

        except Exception as e:
            raise RuntimeError(f"❌ 电机初始化失败：{e}")

    def switch_control_mode(self, control_type):
        """
        切换电机控制模式
        :param control_type: 控制模式（Control_Type.POS_VEl/VEL/MIT）
        :return: 切换成功返回True，否则返回False
        """
        try:
            result = self.motor_control.switchControlMode(self.motor, control_type)
            mode_name = self._get_mode_name(control_type)
            if result:
                print(f"✅ 切换到{mode_name}模式成功")
                # 切换模式后保存参数
                self.motor_control.save_motor_param(self.motor)
            else:
                print(f"❌ 切换到{mode_name}模式失败")
            return result
        except Exception as e:
            print(f"❌ 切换模式出错：{str(e)}")
            return False

    def enable_motor(self):
        """使能电机"""
        try:
            self.motor_control.enable(self.motor)
            print("✅ 电机使能成功")
            return True
        except Exception as e:
            print(f"❌ 电机使能失败：{str(e)}")
            return False

    def disable_motor(self):
        """失能电机"""
        try:
            self.motor_control.disable(self.motor)
            print("✅ 电机失能成功")
            return True
        except Exception as e:
            print(f"❌ 电机失能失败：{str(e)}")
            return False

    def control_pos_vel(self, p_desired, v_desired):
        """
        位置-速度模式控制
        :param p_desired: 目标位置（rad, 范围[-300, 300]）
        :param v_desired: 目标速度（rad/s, 范围[-30, 30]）
        """
        try:
            # 发送运动指令
            # self.motor_control.set_zero_position(self.motor)
            self.motor_control.control_Pos_Vel(self.motor, p_desired, v_desired)
            time.sleep(0.1)
            print(f"✅ 位置-速度控制：位置={p_desired}rad | 速度={v_desired}rad/s")
            return True

        except Exception as e:
            print(f"❌ 位置-速度控制出错：{str(e)}")
            return False

    def close_serial(self):
        """关闭串口"""
        try:
            if self.serial_device and self.serial_device.is_open:
                self.serial_device.close()
                print("✅ 串口关闭成功")
            return True
        except Exception as e:
            print(f"❌ 串口关闭失败：{str(e)}")
            return False

    def _get_mode_name(self, control_type):
        """
        获取模式名称
        :param control_type: 控制模式（Control_Type.POS_VEl/VEL/MIT）
        """
        # 1.定义[枚举值--中文名称]的映射字典
        mode_map = {
            Control_Type.POS_VEL: "位置-速度模式",
            Control_Type.VEL: "速度模式",
            Control_Type.MIT: "MIT模式"
        }
        # 2.根据控制模式值获取中文名称   字典方法
        return mode_map.get(control_type, "未知模式")   # “未知模式”默认值

    def save_param(self):
        """保存所有电机参数"""
        try:
            if self.motor is None:
                raise ValueError("电机实例为None，无法保存参数")

            self.motor_control.save_motor_param(self.motor)
            print("电机参数保存成功")
        except Exception as e:
            print(f"❌ 电机参数保存失败：{str(e)}")

    def refresh_motor_status(self):
        """获得电机状态"""
        try:
            if self.motor is None:
                raise ValueError("电机实例为None，无法保存参数")

            self.motor_control.refresh_motor_status(self.motor)
            print("电机状态刷新成功")
        except Exception as e:
            print(f"❌ 电机状态刷新失败：{str(e)}")

    def get_position(self):
        """获取电机位置"""
        try:
            if self.motor is None:
                raise ValueError("电机实例为None，无法保存参数")

            position = self.motor.getPosition()
            print(f"获取电机位置成功，当前位置: {position}")
            return position
        except Exception as e:
            print(f"获取电机位置失败: {str(e)}")

    def change_limit_param(self, motor_type, pmax, vmax, tmax):
        """
        改变电机的PMAX VMAX TMAX
        :param motor_type: 电机的类型
        :param pmax: 电机的PMAX
        :param vmax: 电机的VMAX
        :param tmax: 电机的TAMX
        """
        try:
            self.motor_control.change_limit_param(motor_type, pmax, vmax, tmax)
            print(
                f"电机限位参数修改成功 | 类型: {motor_type} | PMAX: {pmax} | VMAX: {vmax} | TMAX: {tmax}"
            )
        except Exception as e:
            print(f"修改电机限位参数失败: {str(e)}")

    def __del__(self):
        """析构函数：确保程序退出时失能电机、关闭串口"""
        try:
            # 先检查串口是否打开，避免重复操作
            if self.serial_device and self.serial_device.is_open:
                self.disable_motor()
                self.close_serial()
            else:
                # 串口已关闭，无需重复操作，仅打印日志
                print("ℹ️ 串口已关闭，析构函数无需重复释放资源")
        except Exception as e:
            print(f"ℹ️ 析构函数执行警告：{str(e)}")

if __name__ == '__main__':
    # 1.创建电机控制器实例
    motor_controller = DMMotorController(
        slave_id=SLAVE_ID,
        master_id=MASTER_ID,
        port=PORT,
        baudrate=BAUDRATE
    )

    try:
        # 2.切换到位置-速度模式
        motor_controller.switch_control_mode(Control_Type.POS_VEL)

        # 3.使能电机
        motor_controller.enable_motor()

        # 4.循环控制电机
        while True:
            # --------------
            # 测试极限值
            # --------------
            print(f"1当前位置：{motor_controller.get_position()}")
            motor_controller.control_pos_vel(p_desired=282.6, v_desired=30)  # 450mm  665-215
            print(f"2当前位置：{motor_controller.get_position()}")
            time.sleep(20)
            motor_controller.control_pos_vel(p_desired=-282.6, v_desired=30)
            time.sleep(20)

    except KeyboardInterrupt:
        print("\n⚠️ 用户手动停止程序")
        print(f"最终位置：{motor_controller.get_position()}")
    except Exception as e:
        print(f"\n❌ 程序运行出错：{str(e)}")
    finally:
        # 5. 无论是否出错，最终都要失能电机、关闭串口
        motor_controller.disable_motor()
        motor_controller.close_serial()
        print(f"最终位置：{motor_controller.get_position()}")
        print("✅ 程序正常退出")