114 lines
5.2 KiB
Python
114 lines
5.2 KiB
Python
import math
|
||
from DM_CAN import * # 导入自定义电机通信库（包含电机类、控制类、参数定义等）
|
||
import serial
|
||
import time
|
||
|
||
# 创建第一个电机对象
|
||
# 参数说明：
|
||
# DM_Motor_Type.DM4310: 电机型号为 DM4310
|
||
# 0x01: 该电机在 CAN 网络中的节点 ID（必须与实际设置一致）
|
||
# 0x11: 目标地址或功能码（具体含义需参考 DM_CAN 协议文档）
|
||
Motor1 = Motor(DM_Motor_Type.DM4310, 0x01, 0x11)
|
||
|
||
# 创建第二个电机对象
|
||
# 0x05: 第二个电机的 CAN ID
|
||
# 0x15: 对应的目标地址
|
||
Motor2 = Motor(DM_Motor_Type.DM4310, 0x02, 0x12)
|
||
|
||
# 打开串口设备
|
||
# 'COM8': 串口端口号（Windows 下常见，Linux 可能是 /dev/ttyUSB0）
|
||
# 921600: 波特率，必须与电机驱动器设置一致（高波特率保证实时性）
|
||
# timeout=0.5: 读取超时时间为 0.5 秒，防止程序卡死
|
||
serial_device = serial.Serial('/dev/ttyACM0', 921600, timeout=0.5)
|
||
|
||
# 创建电机控制器对象，负责管理多个电机的通信
|
||
MotorControl1 = MotorControl(serial_device)
|
||
# 将 Motor1 添加到控制器管理列表中
|
||
MotorControl1.addMotor(Motor1)
|
||
# 将 Motor2 添加到控制器管理列表中
|
||
MotorControl1.addMotor(Motor2)
|
||
|
||
# 切换 Motor1 的控制模式为“位置+速度”混合模式
|
||
# 该模式下可同时发送位置和速度指令
|
||
#if MotorControl1.switchControlMode(Motor1, Control_Type.POS_VEL):
|
||
# print("switch POS_VEL success") # 切换成功提示
|
||
if MotorControl1.switchControlMode(Motor1, Control_Type.VEL):
|
||
print("switch VEL success") # 切换成功提示
|
||
# 切换 Motor2 的控制模式为“纯速度”模式
|
||
if MotorControl1.switchControlMode(Motor2, Control_Type.VEL):
|
||
print("switch VEL success") # 切换成功提示
|
||
|
||
# 读取 Motor1 的固件子版本号（sub_ver），用于确认电机固件版本
|
||
print("sub_ver:", MotorControl1.read_motor_param(Motor1, DM_variable.sub_ver))
|
||
# 读取 Motor1 的减速比（Gr），影响实际输出速度和位置
|
||
print("Gr:", MotorControl1.read_motor_param(Motor1, DM_variable.Gr))
|
||
|
||
# 示例：写入参数（当前被注释）
|
||
# 将 Motor1 的位置环比例增益（KP_APR）设为 54
|
||
# if MotorControl1.change_motor_param(Motor1, DM_variable.KP_APR, 54):
|
||
# print("write success")
|
||
|
||
# 读取 Motor1 的最大允许功率（PMAX）
|
||
print("PMAX:", MotorControl1.read_motor_param(Motor1, DM_variable.PMAX))
|
||
# 读取 Motor1 的主控 ID（MST_ID），用于组网识别
|
||
print("MST_ID:", MotorControl1.read_motor_param(Motor1, DM_variable.MST_ID))
|
||
# 读取 Motor1 的最大速度限制（VMAX）
|
||
print("VMAX:", MotorControl1.read_motor_param(Motor1, DM_variable.VMAX))
|
||
# 读取 Motor1 的最大扭矩限制（TMAX）
|
||
print("TMAX:", MotorControl1.read_motor_param(Motor1, DM_variable.TMAX))
|
||
# 输出 Motor2 的参数信息
|
||
print("Motor2:")
|
||
# 读取 Motor2 的最大功率
|
||
print("PMAX:", MotorControl1.read_motor_param(Motor2, DM_variable.PMAX))
|
||
# 读取 Motor2 的主控 ID
|
||
print("MST_ID:", MotorControl1.read_motor_param(Motor2, DM_variable.MST_ID))
|
||
# 读取 Motor2 的最大速度
|
||
print("VMAX:", MotorControl1.read_motor_param(Motor2, DM_variable.VMAX))
|
||
# 读取 Motor2 的最大扭矩
|
||
print("TMAX:", MotorControl1.read_motor_param(Motor2, DM_variable.TMAX))
|
||
|
||
# 将 Motor1 的当前参数保存到电机内部 Flash，掉电不丢失
|
||
MotorControl1.save_motor_param(Motor1)
|
||
# 将 Motor2 的当前参数保存到 Flash
|
||
MotorControl1.save_motor_param(Motor2)
|
||
# 发送使能指令，激活 Motor1（使能后电机进入运行状态，可能带电）
|
||
MotorControl1.enable(Motor1)
|
||
# 发送使能指令，激活 Motor2
|
||
MotorControl1.enable(Motor2)
|
||
# 初始化循环计数器
|
||
i = 0
|
||
# 开始主控制循环，共执行 10000 次（约持续 10 秒，每步延时 1ms）
|
||
while i < 10000:
|
||
# 生成正弦波轨迹：q ∈ [-1, 1]，频率约 0.16 Hz（周期 ~6.28 秒）
|
||
# 使用 time.time() 作为相位，连续变化
|
||
q = math.sin(time.time())
|
||
# 计数器递增
|
||
i = i + 1
|
||
# 控制 Motor1：位置+速度模式
|
||
# 目标位置 = q * 8 （单位：圈）
|
||
# 目标速度 = 30 （单位：转/分钟 或 rpm，具体看协议）
|
||
MotorControl1.control_Vel(Motor2, 8 * q)
|
||
# 控制 Motor2：纯速度模式
|
||
# 目标速度 = 8 * q （单位同上）
|
||
MotorControl1.control_Vel(Motor2, 8 * q)
|
||
# 延时 1 毫秒，控制循环频率约为 1000Hz
|
||
# 注意：Windows 下实际精度可能为 1~15ms，非严格实时
|
||
time.sleep(0.001)
|
||
# 以下为调试用代码（当前被注释）
|
||
# 打印 Motor1 的实时反馈：位置、速度、扭矩
|
||
# print("Motor1:","POS:",Motor1.getPosition(),"VEL:",Motor1.getVelocity(),"TORQUE:",Motor1.getTorque())
|
||
# 打印 Motor2 的实时反馈
|
||
# print("Motor2:","POS:",Motor2.getPosition(),"VEL:",Motor2.getVelocity(),"TORQUE:",Motor2.getTorque())
|
||
# 打印当前扭矩值
|
||
# print(Motor1.getTorque())
|
||
# print(Motor2.getTorque())
|
||
# 示例：MIT 模式控制（力矩控制），当前未使用
|
||
# MotorControl1.controlMIT(Motor2, 35, 0.1, 8*q, 0, 0)
|
||
# 示例：控制第三个电机（未定义）
|
||
# MotorControl1.control(Motor3, 50, 0.3, q, 0, 0)
|
||
|
||
# 循环结束，所有控制指令已完成
|
||
# 关闭串口，释放资源
|
||
# 非常重要！避免端口占用和通信异常
|
||
serial_device.close()
|
||
# 程序结束 |