from motorbridge import (
    # ===== 核心类 =====
    Controller,    # 控制器 - 管理通信和电机
    Motor,         # 电机 - 控制单个电机

    # ===== 枚举和数据类 =====
    Mode,          # 模式枚举 - 控制模式
    MotorState,    # 状态数据类 - 电机状态

    # ===== 寄存器工具 =====
    RegisterSpec,  # 寄存器规格
    DAMIAO_RW_REGISTERS,       # 达妙可读写寄存器字典
    DAMIAO_HIGH_IMPACT_RIDS,   # 高影响寄存器 ID 列表
    DAMIAO_PROTECTION_RIDS,    # 保护寄存器 ID 列表
    get_damiao_register_spec,  # 获取寄存器规格函数

    # ===== 寄存器 ID 常量 =====
    RID_CTRL_MODE,  # 10 - 控制模式
    RID_MST_ID,     # 7  - 反馈 ID
    RID_ESC_ID,     # 8  - 电机 ID
    RID_TIMEOUT,    # 9  - 通信超时
    RID_PMAX,       # 21 - 位置映射范围
    RID_VMAX,       # 22 - 速度映射范围
    RID_TMAX,       # 23 - 力矩映射范围
    RID_KP_ASR,     # 25 - 速度环 Kp
    RID_KI_ASR,     # 26 - 速度环 Ki
    RID_KP_APR,     # 27 - 位置环 Kp
    RID_KI_APR,     # 28 - 位置环 Ki

    # ===== 模式常量 =====
    MODE_MIT,       # 1 - MIT 模式
    MODE_POS_VEL,   # 2 - 位置-速度模式
    MODE_VEL,       # 3 - 速度模式
    MODE_FORCE_POS, # 4 - 力位置模式
)

from motorbridge.errors import (
    MotorBridgeError,  # 基础错误类
    AbiLoadError,      # ABI 加载错误
    CallError,         # API 调用错误
)

from motorbridge import Controller

# 基本用法
ctrl = Controller("can0")

# 使用 with 语句（推荐）
with Controller("can0") as ctrl:
    motor = ctrl.add_damiao_motor(0x01, 0x11, "4340P")
    # ... 控制代码

from motorbridge import Controller

with Controller.from_socketcanfd("can0") as ctrl:
    # Hexfellow 必须用 CAN-FD
    motor = ctrl.add_hexfellow_motor(0x01, 0x00, "hexfellow")

from motorbridge import Controller

with Controller.from_dm_serial("/dev/ttyACM0", baud=921600) as ctrl:
    motor = ctrl.add_damiao_motor(0x01, 0x11, "4340P")

motor = ctrl.add_damiao_motor(
    motor_id=0x01,      # 命令 ID
    feedback_id=0x11,   # 反馈 ID
    model="4340P"       # 型号
)

motor = ctrl.add_robstride_motor(
    motor_id=127,       # 设备 ID
    feedback_id=0xFD,   # host_id，上位机侧 ID，不是电机 ID
    model="rs-06"       # 型号
)

motor = ctrl.add_myactuator_motor(
    motor_id=1,         # 电机 ID
    feedback_id=0x241,  # 反馈 ID = 0x240 + motor_id
    model="X8"          # 型号
)

motor = ctrl.add_hightorque_motor(
    motor_id=1,         # 电机 ID
    feedback_id=0x01,   # 固定为 0x01
    model="hightorque"  # 型号
)

# 必须用 CAN-FD
with Controller.from_socketcanfd("can0") as ctrl:
    motor = ctrl.add_hexfellow_motor(
        motor_id=0x01,
        feedback_id=0x00,  # 固定为 0x00
        model="hexfellow"
    )

ctrl.enable_all()
# 所有电机现在可以响应命令

ctrl.disable_all()
# 所有电机输出关闭，转子自由

# v0.1.6 及更早
motor.request_feedback()
ctrl.poll_feedback_once()  # 必须调用
state = motor.get_state()

# v0.3.3 通常不需要手动调用

ctrl.shutdown()  # 禁用所有电机并清理

ctrl.close_bus()  # 释放 CAN 接口

ctrl.close()  # 完全清理

motor.enable()  # 电机开始输出扭矩

motor.disable()  # 电机输出关闭

# 检测到错误后清除
if state.status_code != 0:
    motor.clear_error()

motor.set_zero_position()  # 当前位置变成 0

motor.close()  # 释放资源

from motorbridge import Mode

# 切换到 MIT 模式
motor.ensure_mode(Mode.MIT, timeout_ms=1000)

motor.send_mit(
    pos=1.0,    # 目标位置（rad）
    vel=0.0,    # 目标速度（rad/s）
    kp=30.0,    # 位置刚度（Nm/rad）
    kd=1.0,     # 速度阻尼（Nm·s/rad）
    tau=0.0     # 前馈力矩（Nm）
)

motor.send_pos_vel(
    pos=2.0,    # 目标位置（rad）
    vlim=1.5    # 最大速度限制（rad/s）
)

motor.send_vel(vel=2.0)  # 目标速度（rad/s）

motor.send_force_pos(
    pos=0.5,    # 目标位置（rad）
    vlim=1.0,   # 最大速度（rad/s）
    ratio=0.3   # 力比例（0.0 - 1.0）
)

motor.request_feedback()  # 发送反馈请求

state = motor.get_state()

if state:
    print(f"位置: {state.pos}")
else:
    print("未收到反馈")

motor.write_register_u32(rid=10, value=2)  # 写入控制模式

value = motor.get_register_u32(rid=8, timeout_ms=1000)
print(f"电机 ID: {value}")

motor.write_register_f32(rid=21, value=12.566)  # 写入 PMAX

pmax = motor.get_register_f32(rid=21, timeout_ms=1000)
print(f"PMAX: {pmax}")

motor.store_parameters()
print("参数已保存到闪存")

motor.set_can_timeout_ms(5000)  # 5 秒超时

device_id, responder_id = motor.robstride_ping()
print(f"设备 ID: {device_id}, 响应者 ID: {responder_id}")

motor.robstride_set_device_id(42)

# 写入不同类型的参数
motor.robstride_write_param_i8(param_id, value)    # 有符号 8 位
motor.robstride_write_param_u8(param_id, value)    # 无符号 8 位
motor.robstride_write_param_u16(param_id, value)   # 无符号 16 位
motor.robstride_write_param_u32(param_id, value)   # 无符号 32 位
motor.robstride_write_param_f32(param_id, value)   # 32 位浮点

# 读取不同类型的参数
v_i8  = motor.robstride_get_param_i8(param_id, timeout_ms=1000)
v_u8  = motor.robstride_get_param_u8(param_id, timeout_ms=1000)
v_u16 = motor.robstride_get_param_u16(param_id, timeout_ms=1000)
v_u32 = motor.robstride_get_param_u32(param_id, timeout_ms=1000)
v_f32 = motor.robstride_get_param_f32(param_id, timeout_ms=1000)

from motorbridge import Mode

class Mode(IntEnum):
    MIT = 1        # MIT 模式 - 全阻抗控制，最灵活
    POS_VEL = 2    # 位置-速度模式 - 简单位置控制
    VEL = 3        # 速度模式 - 纯速度控制
    FORCE_POS = 4  # 力位置模式 - 柔顺位置控制

from motorbridge import Mode

# 切换模式
motor.ensure_mode(Mode.MIT, 1000)

# 检查模式值
if Mode.MIT == 1:
    print("MIT 模式的值是 1")

# 遍历所有模式
for mode in Mode:
    print(f"{mode.name} = {mode.value}")

from motorbridge import MODE_MIT, MODE_POS_VEL, MODE_VEL, MODE_FORCE_POS

# 这些值与 Mode 枚举相同
assert MODE_MIT == Mode.MIT == 1
assert MODE_POS_VEL == Mode.POS_VEL == 2
assert MODE_VEL == Mode.VEL == 3
assert MODE_FORCE_POS == Mode.FORCE_POS == 4

from motorbridge import MotorState

@dataclass(frozen=True)
class MotorState:
    can_id: int          # 电机 CAN ID
    arbitration_id: int  # CAN 仲裁 ID
    status_code: int     # 状态/错误码
    pos: float           # 位置（rad）
    vel: float           # 速度（rad/s）
    torq: float          # 力矩（Nm）
    t_mos: float         # MOSFET 温度（°C）
    t_rotor: float       # 转子温度（°C）

motor.request_feedback()
state = motor.get_state()

if state:
    # 访问字段
    print(f"位置: {state.pos:.3f} rad")
    print(f"速度: {state.vel:.3f} rad/s")
    print(f"力矩: {state.torq:.3f} Nm")

    # 检查状态码
    if state.status_code != 0:
        print(f"错误码: 0x{state.status_code:02X}")

    # 检查温度
    if state.t_mos > 70:
        print("温度过高！")

from motorbridge.errors import MotorBridgeError, AbiLoadError, CallError

MotorBridgeError (基类)
├── AbiLoadError    # ABI 库加载失败
└── CallError       # API 调用失败

from motorbridge.errors import MotorBridgeError

try:
    # ... motorbridge 操作
    pass
except MotorBridgeError as e:
    print(f"MotorBridge 错误: {e}")

from motorbridge.errors import AbiLoadError

try:
    from motorbridge import Controller
except AbiLoadError as e:
    print(f"无法加载底层库: {e}")
    print("请检查 libmotor_abi 是否正确安装")

from motorbridge.errors import CallError

try:
    motor.send_mit(1.0, 0.0, 30.0, 1.0, 0.0)
except CallError as e:
    print(f"API 调用失败: {e}")
    # 可能原因：电机未使能、通信超时等

from motorbridge import Controller, Mode
from motorbridge.errors import MotorBridgeError, CallError

try:
    with Controller("can0") as ctrl:
        motor = ctrl.add_damiao_motor(0x01, 0x11, "4340P")
        ctrl.enable_all()
        motor.ensure_mode(Mode.MIT, 1000)

        for i in range(100):
            motor.send_mit(0.5, 0.0, 30.0, 1.0, 0.0)

except CallError as e:
    print(f"API 调用失败: {e}")
    # 处理 API 错误

except MotorBridgeError as e:
    print(f"MotorBridge 错误: {e}")
    # 处理一般错误

except KeyboardInterrupt:
    print("\n用户中断")
    # 清理

finally:
    # 确保资源清理
    pass

from motorbridge import RegisterSpec

@dataclass(frozen=True)
class RegisterSpec:
    rid: int          # 寄存器 ID
    variable: str     # 变量名
    description: str  # 人类可读描述
    data_type: str    # 数据类型："f32" 或 "u32"
    access: str       # 访问权限："RW", "RO" 等
    range_str: str    # 范围描述

from motorbridge import DAMIAO_RW_REGISTERS, get_damiao_register_spec

# 方法1：从字典获取
spec = DAMIAO_RW_REGISTERS[21]  # PMAX
print(f"RID {spec.rid}: {spec.variable}")
print(f"  描述: {spec.description}")
print(f"  类型: {spec.data_type}")
print(f"  访问: {spec.access}")
print(f"  范围: {spec.range_str}")

# 方法2：使用辅助函数
spec = get_damiao_register_spec(21)
if spec:
    print(f"找到: {spec.variable}")
else:
    print("寄存器不存在")

from motorbridge import (
    # ID 相关
    RID_MST_ID,     # 7  - 反馈 ID（MST_ID）
    RID_ESC_ID,     # 8  - 电机 ID（ESC_ID）

    # 通信相关
    RID_TIMEOUT,    # 9  - 通信超时（毫秒）

    # 模式相关
    RID_CTRL_MODE,  # 10 - 控制模式

    # 限制相关
    RID_PMAX,       # 21 - 位置映射范围
    RID_VMAX,       # 22 - 速度映射范围
    RID_TMAX,       # 23 - 力矩映射范围

    # PID 相关
    RID_KP_ASR,     # 25 - 速度环 Kp
    RID_KI_ASR,     # 26 - 速度环 Ki
    RID_KP_APR,     # 27 - 位置环 Kp
    RID_KI_APR,     # 28 - 位置环 Ki
)

#!/usr/bin/env python3
"""motorbridge 完整模板"""

import time
from motorbridge import (
    Controller,
    Mode,
    MotorState,
    RID_PMAX,
    RID_VMAX,
    RID_TMAX,
    DAMIAO_RW_REGISTERS,
    get_damiao_register_spec,
)
from motorbridge.errors import CallError, MotorBridgeError

# ============ 配置区域 ============
CHANNEL = "can0"
MOTOR_ID = 0x01
FEEDBACK_ID = 0x11
MODEL = "4340P"
MODE = Mode.MIT
TARGET_POS = 1.0
CONTROL_RATE = 50  # Hz
DURATION = 2.0     # 秒
# =================================

def main():
    try:
        with Controller(CHANNEL) as ctrl:
            # ===== 注册电机 =====
            motor = ctrl.add_damiao_motor(MOTOR_ID, FEEDBACK_ID, MODEL)
            print(f"电机已注册: ID=0x{MOTOR_ID:02X}")

            # ===== 使能 =====
            ctrl.enable_all()
            print("电机已使能")

            # ===== 设置模式 =====
            motor.ensure_mode(MODE, timeout_ms=1000)
            print(f"模式已设置: {MODE.name}")

            # ===== 读取配置 =====
            pmax = motor.get_register_f32(RID_PMAX, 1000)
            vmax = motor.get_register_f32(RID_VMAX, 1000)
            tmax = motor.get_register_f32(RID_TMAX, 1000)
            print(f"限制: PMAX={pmax:.3f}, VMAX={vmax:.3f}, TMAX={tmax:.3f}")

            # ===== 控制循环 =====
            dt = 1.0 / CONTROL_RATE
            iterations = int(DURATION / dt)

            for i in range(iterations):
                t0 = time.time()

                # 发送命令
                motor.send_mit(TARGET_POS, 0.0, 30.0, 1.0, 0.0)

                # 读取状态
                motor.request_feedback()
                state = motor.get_state()

                if state:
                    print(
                        f"#{i:03d} "
                        f"pos={state.pos:+.3f} "
                        f"vel={state.vel:+.3f} "
                        f"torq={state.torq:+.2f} "
                        f"t_mos={state.t_mos:.1f}°C"
                    )
                else:
                    print(f"#{i:03d} 未收到反馈")

                # 保持固定频率
                elapsed = time.time() - t0
                if elapsed < dt:
                    time.sleep(dt - elapsed)

            # ===== 清理 =====
            motor.disable()
            print("电机已禁用")

    except CallError as e:
        print(f"API 调用失败: {e}")
    except MotorBridgeError as e:
        print(f"MotorBridge 错误: {e}")
    except KeyboardInterrupt:
        print("\n用户中断")

if __name__ == "__main__":
    main()