#!/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)