你想做什么？
│
├── 精确控制位置，想调整"硬度"
│   └── MIT 模式
│
├── 简单的点到点运动
│   └── POS_VEL 模式
│
├── 让电机一直转（传送带、轮子）
│   └── VEL 模式
│
└── 做夹爪，需要力控制
    └── FORCE_POS 模式

from motorbridge import Controller, Mode

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

# 切换到位置-速度模式
motor.ensure_mode(Mode.POS_VEL, timeout_ms=1000)

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

# 切换到力位置模式
motor.ensure_mode(Mode.FORCE_POS, timeout_ms=1000)

motor.ensure_mode(Mode.MIT, 1000)

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

# 硬：位置控制，响应快
motor.send_mit(
    pos=1.0,
    vel=0.0,
    kp=50.0,   # 高刚度
    kd=2.0,    # 适度阻尼
    tau=0.0
)

# 软：柔顺控制，可以推动
motor.send_mit(
    pos=1.0,
    vel=0.0,
    kp=5.0,    # 低刚度
    kd=0.5,    # 低阻尼
    tau=0.0
)

# 加入速度前馈，让运动更平滑
import math
import time

for i in range(100):
    t = i * 0.02
    target_pos = math.sin(t)       # 目标位置
    target_vel = math.cos(t)       # 目标速度（导数）

    motor.send_mit(
        pos=target_pos,
        vel=target_vel,  # 速度前馈
        kp=30.0,
        kd=1.0,
        tau=0.0
    )
    time.sleep(0.02)

# 只用力矩，不用位置控制
motor.send_mit(
    pos=0.0,
    vel=0.0,
    kp=0.0,    # 零刚度
    kd=0.0,    # 零阻尼
    tau=0.5    # 0.5Nm 力矩
)

motor.ensure_mode(Mode.POS_VEL, 1000)

motor.send_pos_vel(
    pos=2.0,    # 目标位置（弧度）
    vlim=1.5    # 最大速度（弧度/秒）
)

import time

# 移动到 2 弧度位置，速度不超过 1.5 rad/s
motor.send_pos_vel(pos=2.0, vlim=1.5)

# 等待到达
for i in range(100):
    motor.request_feedback()
    state = motor.get_state()

    if state:
        error = abs(state.pos - 2.0)
        print(f"位置误差: {error:.4f} rad")

        if error < 0.02:  # 误差小于 0.02 rad 认为到达
            print("到达目标！")
            break

    time.sleep(0.02)

motor.ensure_mode(Mode.VEL, 1000)

motor.send_vel(vel=2.0)  # 2 rad/s

import time

# 加速
for v in range(0, 30):
    motor.send_vel(vel=v * 0.1)  # 0 到 3 rad/s
    time.sleep(0.05)

# 保持
time.sleep(2.0)

# 减速
for v in range(30, 0, -1):
    motor.send_vel(vel=v * 0.1)
    time.sleep(0.05)

# 停止
motor.send_vel(vel=0.0)

motor.ensure_mode(Mode.FORCE_POS, 1000)

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

import time

def gripper_open(motor):
    """打开夹爪"""
    motor.send_force_pos(pos=1.5, vlim=2.0, ratio=0.5)

def gripper_close(motor, force=0.3):
    """关闭夹爪"""
    motor.send_force_pos(pos=-0.5, vlim=1.0, ratio=force)

# 打开
gripper_open(motor)
time.sleep(1.0)

# 轻轻夹
gripper_close(motor, force=0.2)
time.sleep(1.0)

# 用力夹
gripper_close(motor, force=0.6)
time.sleep(0.5)

# 释放
gripper_open(motor)

import time

DT = 0.02  # 20ms = 50Hz

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

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

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

    if state:
        print(f"#{i:03d} pos={state.pos:.3f}")

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

import time

TARGET = 1.0
TOLERANCE = 0.02  # 0.02 rad

motor.ensure_mode(Mode.POS_VEL, 1000)
motor.send_pos_vel(TARGET, vlim=1.5)

while True:
    motor.request_feedback()
    state = motor.get_state()

    if state:
        error = abs(state.pos - TARGET)
        print(f"误差: {error:.4f} rad")

        if error < TOLERANCE:
            print("到达目标！")
            break

    time.sleep(0.02)

#!/usr/bin/env python3
"""完整控制示例"""

import time
import math
from motorbridge import Controller, Mode

def main():
    with Controller("can0") as ctrl:
        motor = ctrl.add_damiao_motor(0x01, 0x11, "4340P")

        print("使能电机...")
        ctrl.enable_all()

        # ===== MIT 模式演示 =====
        print("\n--- MIT 模式 ---")
        motor.ensure_mode(Mode.MIT, 1000)

        print("硬位置控制")
        for _ in range(50):
            motor.send_mit(0.5, 0.0, 50.0, 2.0, 0.0)
            time.sleep(0.02)

        print("柔顺控制")
        for _ in range(50):
            motor.send_mit(0.0, 0.0, 5.0, 0.5, 0.0)
            time.sleep(0.02)

        # ===== POS_VEL 模式演示 =====
        print("\n--- POS_VEL 模式 ---")
        motor.ensure_mode(Mode.POS_VEL, 1000)

        for pos in [1.0, -1.0, 0.0]:
            print(f"移动到 {pos:.1f} rad")
            motor.send_pos_vel(pos, vlim=1.5)
            time.sleep(1.0)

        # ===== VEL 模式演示 =====
        print("\n--- VEL 模式 ---")
        motor.ensure_mode(Mode.VEL, 1000)

        print("旋转 2 rad/s")
        for _ in range(50):
            motor.send_vel(2.0)
            time.sleep(0.02)

        print("停止")
        motor.send_vel(0.0)

        print("\n演示完成")

if __name__ == "__main__":
    main()