# 检查 CAN 接口是否存在
ip link show can0

# 如果不存在，配置它
sudo ip link set can0 type can bitrate 1000000
sudo ip link set can0 up

# 增加 TX 队列长度（防止缓冲区溢出）
sudo ifconfig can0 txqueuelen 1000

motorbridge-cli scan --vendor all --channel can0 --start-id 1 --end-id 255

# 只扫描达妙电机
motorbridge-cli scan --vendor damiao --channel can0 --start-id 1 --end-id 32

# 只扫描 RobStride 电机
motorbridge-cli scan --vendor robstride --channel can0 --start-id 1 --end-id 127

# 只扫描 MyActuator 电机
motorbridge-cli scan --vendor myactuator --channel can0 --start-id 1 --end-id 32

command=scan vendor=damiao channel=can0 id_range=[0x1,0x20] timeout_ms=80

[scan:damiao] channel=can0 model=4340 id_range=[0x1,0x20]
[hit] vendor=damiao probe=0x01 esc_id=0x1 mst_id=0x11
[hit] vendor=damiao probe=0x02 esc_id=0x2 mst_id=0x12
[.. ] vendor=damiao probe=0x03 no reply

scan done: 2 motor(s) found
  probe=0x01 vendor=damiao esc_id=0x1 mst_id=0x11
  probe=0x02 vendor=damiao esc_id=0x2 mst_id=0x12

# motor_config.py - 根据扫描结果填写

MOTORS = {
    "左髋关节": {
        "vendor": "damiao",
        "model": "4340P",
        "motor_id": 0x01,      # 从扫描结果获取
        "feedback_id": 0x11,   # 从扫描结果获取
    },
    "左膝关节": {
        "vendor": "damiao",
        "model": "4340P",
        "motor_id": 0x02,
        "feedback_id": 0x12,
    },
}

import time
from motorbridge import Controller

def scan_damiao_motors(channel, start_id, end_id):
    """
    扫描达妙电机。
    
    参数:
        channel: CAN 接口名，如 "can0"
        start_id: 起始 ID，如 1
        end_id: 结束 ID，如 32
    
    返回:
        找到的电机列表，每个元素是 (motor_id, feedback_id) 元组
    """
    found_motors = []
    
    print(f"开始扫描 {channel}，ID 范围: {start_id} - {end_id}")
    
    for motor_id in range(start_id, end_id + 1):
        # 达妙的 feedback_id 通常是 motor_id + 0x10
        feedback_id = 0x10 + (motor_id & 0x0F)
        
        # 创建新的控制器实例
        ctrl = Controller(channel)
        
        try:
            # 尝试添加电机
            motor = ctrl.add_damiao_motor(motor_id, feedback_id, "4340P")
            
            try:
                # 尝试读取寄存器来验证电机存在
                # RID 8 是 ESC_ID（电机 ID）
                esc_id = motor.get_register_u32(8, timeout_ms=100)
                # RID 7 是 MST_ID（反馈 ID）
                mst_id = motor.get_register_u32(7, timeout_ms=100)
                
                print(f"[找到] motor_id=0x{motor_id:02X} esc=0x{esc_id:X} mst=0x{mst_id:X}")
                found_motors.append((motor_id, feedback_id))
                
            except Exception:
                # 读取失败，说明这个 ID 没有电机
                print(f"[无响应] motor_id=0x{motor_id:02X}")
                
            finally:
                motor.close()
                
        except Exception as e:
            print(f"[错误] motor_id=0x{motor_id:02X}: {e}")
            
        finally:
            ctrl.close_bus()
            ctrl.close()
    
    print(f"\n扫描完成，找到 {len(found_motors)} 个电机")
    return found_motors

# 运行扫描
if __name__ == "__main__":
    motors = scan_damiao_motors("can0", 1, 20)
    
    print("\n找到的电机配置:")
    for motor_id, feedback_id in motors:
        print(f"  motor_id=0x{motor_id:02X}, feedback_id=0x{feedback_id:02X}")

motor_id:    0x01 - 0x20 (1-32)
feedback_id: motor_id + 0x10

示例:
  motor_id = 0x01 → feedback_id = 0x11
  motor_id = 0x0A → feedback_id = 0x1A
  motor_id = 0x10 → feedback_id = 0x20

motor_id:    1-127 (通常默认是 127)
feedback_id: 0xFE 或 0xFF

示例:
  motor_id = 127, feedback_id = 0xFE

motor_id:    1-32
feedback_id: 0x240 + motor_id

示例:
  motor_id = 1  → feedback_id = 0x241
  motor_id = 8  → feedback_id = 0x248

motor_id:    1-127
feedback_id: 0x01 (固定)

motor_id:    0x01 - 0xFF
feedback_id: 0x00 (固定)
注意: Hexfellow 必须用 CAN-FD

motorbridge-cli scan \
    --vendor damiao \
    --transport dm-serial \
    --serial-port /dev/ttyACM0 \
    --serial-baud 921600 \
    --start-id 1 \
    --end-id 32

# 逐个上电扫描
# 给每个电机分配不同的 ID

motorbridge-cli scan --timeout-ms 200 ...

# 只扫描 1-10
motorbridge-cli scan --start-id 1 --end-id 10 ...

#!/usr/bin/env python3
"""完整的电机发现和控制示例"""

from motorbridge import Controller, Mode

# 第一步：扫描电机（假设我们已经通过 CLI 扫描过了）
# 扫描结果：motor_id=0x01, feedback_id=0x11

# 第二步：配置电机
MOTOR_ID = 0x01
FEEDBACK_ID = 0x11
MODEL = "4340P"

# 第三步：创建控制器
with Controller("can0") 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.MIT, timeout_ms=1000)
    print("已设置为 MIT 模式")
    
    # 第七步：控制电机
    print("开始控制...")
    for i in range(10):
        motor.send_mit(0.5, 0.0, 30.0, 1.0, 0.0)
        
        motor.request_feedback()
        state = motor.get_state()
        if state:
            print(f"位置: {state.pos:.3f} rad")
        
        import time
        time.sleep(0.02)
    
    # 第八步：关闭电机
    motor.disable()
    print("电机已关闭")