from motorbridge import Controller, Mode

# 打开 CAN 接口
with Controller("can0") as ctrl:
    # 添加达妙电机
    motor = ctrl.add_damiao_motor(0x01, 0x11, "4340P")
    
    # 使能并配置
    ctrl.enable_all()
    motor.ensure_mode(Mode.MIT, 1000)
    
    # 发送控制命令
    motor.send_mit(pos=0.5, vel=0.0, kp=30.0, kd=1.0, tau=0.0)
    
    # 读取状态
    state = motor.get_state()
    if state:
        print(f"位置: {state.pos:.3f} rad")