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()
# 程序结束