257 lines
9.0 KiB
Python
257 lines
9.0 KiB
Python
|
#!/usr/bin/env python
|
|||
|
|
# -*- coding: utf-8 -*-
|
|||
|
|
'''
|
|||
|
|
# @Time : 2026/1/6 13:55
|
|||
|
|
# @Author : reenrr
|
|||
|
|
# @File : DM_Motor_test.py
|
|||
|
|
# @Desc : 达妙电机测试
|
|||
|
|
'''
|
|||
|
|
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)}")
|
|||
|
|
|
|||
|
|
|
|||
|
|
def dm_motor_control():
|
|||
|
|
# 1.创建电机控制器实例
|
|||
|
|
motor_controller = DMMotorController(
|
|||
|
|
slave_id=SLAVE_ID,
|
|||
|
|
master_id=MASTER_ID,
|
|||
|
|
port=PORT,
|
|||
|
|
baudrate=BAUDRATE
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
# 切换到位置-速度模式
|
|||
|
|
motor_controller.switch_control_mode(Control_Type.POS_VEL)
|
|||
|
|
|
|||
|
|
# 使能电机
|
|||
|
|
motor_controller.enable_motor()
|
|||
|
|
|
|||
|
|
# 循环控制电机
|
|||
|
|
while True:
|
|||
|
|
print("运动前的位置", motor_controller.get_position()) # 需要测试断电后是否能读取得到
|
|||
|
|
motor_controller.control_pos_vel(p_desired=282.6, v_desired=30) # 450mm 665-215
|
|||
|
|
time.sleep(20)
|
|||
|
|
|
|||
|
|
motor_controller.refresh_motor_status()
|
|||
|
|
print("运动1的位置", motor_controller.get_position()) # 刷新的比较慢，可以等位置不变一段时间之后，再获取位置
|
|||
|
|
motor_controller.refresh_motor_status()
|
|||
|
|
|
|||
|
|
motor_controller.control_pos_vel(p_desired=-282.6, v_desired=30)
|
|||
|
|
time.sleep(20)
|
|||
|
|
|
|||
|
|
|
|||
|
|
except KeyboardInterrupt:
|
|||
|
|
print("\n⚠️ 用户手动停止程序")
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"\n❌ 程序运行出错：{str(e)}")
|
|||
|
|
finally:
|
|||
|
|
# 5. 无论是否出错，最终都要失能电机、关闭串口
|
|||
|
|
motor_controller.disable_motor()
|
|||
|
|
motor_controller.close_serial()
|
|||
|
|
print("✅ 程序正常退出")
|
|||
|
|
|
|||
|
|
# ---------调试接口----------
|
|||
|
|
if __name__ == '__main__':
|
|||
|
|
dm_motor_control()
|