修改传送带电机、舵机、达妙电机等相关接口

2026-02-26 15:42:22 +08:00
parent 5c9946dfc7
commit 8d2e91b8b3
10 changed files with 1642 additions and 331 deletions
--- a/DM/DM_Motor_controller.py
+++ b/DM/DM_Motor_controller.py
@ -0,0 +1,247 @@
+#!/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()
+
+    set_dm_motor_speed(30)
+    set_ori_position(282.6)
+    move_to_ori_position()