digital PID controller implementation
You are of by one order of magnitude. \$a_2 = \frac{K_d}{T} \rightarrow T = \frac{K_d}{a_2} = \frac{0.005}{500} = 10^{-5}\$ and not (your) \$T = 10^{-6}\$ so your values need to be adjusted.