Do this before you try to implement your PID.

I know you want to jump right into PIDs but if you do a little preparation up front, your implementation will go a lot smoother.