260 lines
8.8 KiB
Python
260 lines
8.8 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 *
|
||
from DM_Motor import DMMotorController
|
||
|
||
import time
|
||
|
||
# -------------------------- 电机参数配置 --------------------------
|
||
SLAVE_ID = 0x01
|
||
MASTER_ID = 0x11
|
||
PORT = 'COM10'
|
||
BAUDRATE = 921600
|
||
|
||
ORI_P_DESIRED = 0.0 # 吸附位置
|
||
DROP_P_DESIRED = 0.0 # 放置位置
|
||
V_DESIRED = 30
|
||
P_DESIRED_MIN = -300
|
||
P_DESIRED_MAX = 300
|
||
V_DESIRED_MIN = -30
|
||
V_DESIRED_MAX = 30
|
||
|
||
ORIGIN_POSITION = 0.0 # 电机原点位置
|
||
POSITION_TOLERANCE = 0.1 # 位置误差阈值
|
||
|
||
|
||
class DMMotorInterface:
|
||
def __init__(self):
|
||
"""初始化电机控制器，切换模式并使能电机"""
|
||
self.motor_controller = DMMotorController(
|
||
slave_id=SLAVE_ID,
|
||
master_id=MASTER_ID,
|
||
port=PORT,
|
||
baudrate=BAUDRATE
|
||
)
|
||
|
||
try:
|
||
# 切换到位置-速度模式
|
||
self.motor_controller.switch_control_mode(Control_Type.POS_VEL)
|
||
# 使能电机
|
||
self.motor_controller.enable_motor()
|
||
except Exception as e:
|
||
raise RuntimeError(f"电机初始化失败:{str(e)}") from e
|
||
|
||
# 全局变量
|
||
_DMMotor = DMMotorInterface()
|
||
|
||
def set_dm_motor_speed(value: int) -> bool:
|
||
"""
|
||
设置达妙电机速度
|
||
:param value: 速度值
|
||
:return: 是否设置成功
|
||
"""
|
||
global V_DESIRED_MIN, V_DESIRED_MAX, V_DESIRED
|
||
|
||
try:
|
||
_min = V_DESIRED_MIN
|
||
_max = V_DESIRED_MAX
|
||
except NameError as e:
|
||
err_msg = f"速度限位常量未定义：{str(e)}，请检查V_DESIRED_MIN/V_DESIRED_MAX的定义"
|
||
print(f"错误：{err_msg}")
|
||
raise Exception(err_msg) from e
|
||
|
||
if not (_min <= value <= _max):
|
||
err_msg = f"速度值超出合法范围：{value}，合法范围[{_min}, {_max}]"
|
||
print(f"错误：{err_msg}")
|
||
# 可选：若需要上层感知超限，改为抛ValueError（替代return False）
|
||
# raise ValueError(err_msg)
|
||
return False
|
||
|
||
try:
|
||
old_value = V_DESIRED
|
||
V_DESIRED = value
|
||
print(f"成功：电机速度已更新，旧值={old_value}，新值={value}（范围[{_min}, {_max}]）")
|
||
return True
|
||
|
||
except Exception as e:
|
||
# 5. 异常处理：保留原始异常链，加详细日志
|
||
err_msg = f"设置电机速度失败：{str(e)}（尝试设置的值：{value}）"
|
||
print(f"异常：{err_msg}")
|
||
# 保留原始异常链，便于上层定位根因
|
||
raise Exception(err_msg) from e
|
||
|
||
def set_ori_position(value: float) ->bool:
|
||
"""
|
||
设置吸附位置
|
||
:param value: 设置吸附位置值
|
||
"""
|
||
global ORI_P_DESIRED, P_DESIRED_MIN, P_DESIRED_MAX
|
||
|
||
try:
|
||
_min = P_DESIRED_MIN
|
||
_max = P_DESIRED_MAX
|
||
except NameError as e:
|
||
err_msg = f"位置限位常量未定义：{str(e)}，请检查P_DESIRED_MIN/P_DESIRED_MAX的定义"
|
||
print(f"错误：{err_msg}")
|
||
raise Exception(err_msg) from e
|
||
|
||
if not (_min <= value <= _max):
|
||
err_msg = f"位置值超出合法范围：{value}，合法范围[{_min}, {_max}]"
|
||
print(f"错误：{err_msg}")
|
||
# 可选：若需要上层感知超限，改为抛ValueError（替代return False）
|
||
# raise ValueError(err_msg)
|
||
return False
|
||
|
||
try:
|
||
old_value = ORI_P_DESIRED
|
||
ORI_P_DESIRED = value
|
||
print(f"成功：电机速度已更新，旧值={old_value}，新值={value}（范围[{_min}, {_max}]）")
|
||
return True
|
||
|
||
except Exception as e:
|
||
err_msg = f"设置电机速度失败：{str(e)}（尝试设置的值：{value}）"
|
||
print(f"异常：{err_msg}")
|
||
# 保留原始异常链，便于上层定位根因
|
||
raise Exception(err_msg) from e
|
||
|
||
def set_drop_position(value: float) -> bool:
|
||
"""
|
||
设置放置位置
|
||
:param value: 设置放置位置
|
||
"""
|
||
global DROP_P_DESIRED, P_DESIRED_MIN, P_DESIRED_MAX
|
||
|
||
try:
|
||
_min = P_DESIRED_MIN
|
||
_max = P_DESIRED_MAX
|
||
except NameError as e:
|
||
err_msg = f"位置限位常量未定义：{str(e)}，请检查P_DESIRED_MIN/P_DESIRED_MAX的定义"
|
||
print(f"错误：{err_msg}")
|
||
raise Exception(err_msg) from e
|
||
|
||
if not (_min <= value <= _max):
|
||
err_msg = f"位置值超出合法范围：{value}，合法范围[{_min}, {_max}]"
|
||
print(f"错误：{err_msg}")
|
||
# 可选：若需要上层感知超限，改为抛ValueError（替代return False）
|
||
# raise ValueError(err_msg)
|
||
return False
|
||
|
||
try:
|
||
old_value = DROP_P_DESIRED
|
||
DROP_P_DESIRED = value
|
||
print(f"成功：电机速度已更新，旧值={old_value}，新值={value}（范围[{_min}, {_max}]）")
|
||
return True
|
||
|
||
except Exception as e:
|
||
# 5. 异常处理：保留原始异常链，加详细日志
|
||
err_msg = f"设置电机速度失败：{str(e)}（尝试设置的值：{value}）"
|
||
print(f"异常：{err_msg}")
|
||
# 保留原始异常链，便于上层定位根因
|
||
raise Exception(err_msg) from e
|
||
|
||
def move_to_ori_position():
|
||
"""
|
||
移动到吸附位置
|
||
"""
|
||
try:
|
||
print(f"开始执行--移动到吸附位置：目标位置={ORI_P_DESIRED},速度={V_DESIRED}")
|
||
|
||
_DMMotor.motor_controller.control_pos_vel(ORI_P_DESIRED, V_DESIRED)
|
||
time.sleep(5) # 等待电机移动完成
|
||
print(f"吸附位置移动指令已发送")
|
||
|
||
except Exception as e:
|
||
# 封装异常信息，保留原始异常链
|
||
err_msg = f"移动到吸附位置失败：{str(e)}（目标位置={ORI_P_DESIRED}，速度={V_DESIRED}）"
|
||
print(f"❌ 异常：{err_msg}")
|
||
raise RuntimeError(err_msg) from e
|
||
|
||
def move_to_drop_position():
|
||
"""
|
||
移动到放置位置
|
||
"""
|
||
try:
|
||
print(f"开始执行--移动到放置位置：目标位置={DROP_P_DESIRED},速度={V_DESIRED}")
|
||
|
||
_DMMotor.motor_controller.control_pos_vel(DROP_P_DESIRED, V_DESIRED)
|
||
time.sleep(5) # 等待电机移动完成
|
||
print(f"放置位置移动指令已发送")
|
||
|
||
except Exception as e:
|
||
# 封装异常信息，保留原始异常链
|
||
err_msg = f"移动到放置位置失败：{str(e)}（目标位置={DROP_P_DESIRED}，速度={V_DESIRED}）"
|
||
print(f"❌ 异常：{err_msg}")
|
||
raise RuntimeError(err_msg) from e
|
||
|
||
def set_zero_position() -> bool:
|
||
"""
|
||
电机零点标定：将当前物理位置设置为软件0位
|
||
注意：调用前需确保电机处于稳定的物理零点位置
|
||
"""
|
||
try:
|
||
print("🔍 开始执行电机零点标定...")
|
||
result = _DMMotor.motor_controller.motor_control.set_zero_position(_DMMotor.motor_controller.motor)
|
||
|
||
if result:
|
||
# 保存标定参数（关键：否则断电后零点丢失）
|
||
_DMMotor.motor_controller.save_param()
|
||
print("✅ 零点标定成功！当前位置已设为0位，参数已保存")
|
||
return True
|
||
else:
|
||
print("❌ 零点标定失败！")
|
||
return False
|
||
|
||
except Exception as e:
|
||
err_msg = f"零点标定异常：{str(e)}"
|
||
print(f"❌ {err_msg}")
|
||
raise RuntimeError(err_msg) from e
|
||
|
||
def reset_motor():
|
||
"""复位达妙电机，防止断电"""
|
||
try:
|
||
# 读取断电后的当前位置
|
||
print("开始电机复位流程：读取当前位置...")
|
||
current_pos = _DMMotor.motor_controller.get_position()
|
||
print(f"✅ 读取当前位置成功：{current_pos}")
|
||
|
||
# 计算需要移动的偏移量（核心逻辑）
|
||
if abs(current_pos) <= POSITION_TOLERANCE:
|
||
print(f"ℹ️ 当前位置已在原点附近（误差={abs(current_pos)} ≤ {POSITION_TOLERANCE}），无需复位")
|
||
return True
|
||
|
||
reset_target_p = -current_pos # 重点修改：p_desired = -当前位置
|
||
print(f"📌 复位路径计算：当前位置={current_pos} → 需设置p_desired={reset_target_p} → 回到0位")
|
||
|
||
print(f"🚀 开始复位移动：p_desired={reset_target_p}，速度={V_DESIRED} rpm")
|
||
_DMMotor.motor_controller.control_pos_vel(p_desired=reset_target_p, v_desired=V_DESIRED)
|
||
time.sleep(5) # 等待电机复位
|
||
|
||
except Exception as e:
|
||
err_msg = f"电机复位失败：{str(e)}"
|
||
print(f"❌ 错误：{err_msg}")
|
||
raise RuntimeError(err_msg) from e
|
||
|
||
# ---------调试接口----------
|
||
if __name__ == '__main__':
|
||
# 首先在吸附位置处进行 设置零点位置
|
||
set_zero_position()
|
||
|
||
reset_motor() # 待测试
|
||
|
||
set_dm_motor_speed(30)
|
||
set_drop_position(282.6)
|
||
set_ori_position(-282.6)
|
||
|
||
# 起始位置：吸附位置
|
||
move_to_drop_position()
|
||
time.sleep(5) # 电机运动需要的时间
|
||
move_to_ori_position()
|
||
time.sleep(5) # 电机运动需要的时间
|
||
|
||
while(True):
|
||
time.sleep(1)
|
||
|