from motorbridge import Controller, Mode

# Open CAN interface
with Controller("can0") as ctrl:
    # Add a Damiao motor
    motor = ctrl.add_damiao_motor(0x01, 0x11, "4340P")
    
    # Enable and configure
    ctrl.enable_all()
    motor.ensure_mode(Mode.MIT, 1000)
    
    # Send control command
    motor.send_mit(pos=0.5, vel=0.0, kp=30.0, kd=1.0, tau=0.0)
    
    # Read state
    state = motor.get_state()
    if state:
        print(f"Position: {state.pos:.3f} rad")