So far, we have determined that you did not scope the variables with the current position properly.
So far, we have determined that your PID doesn't really work as a PID because of errors in your software.
Have you fixed those problems yet?
First you need to make sure the current position term is passed to the main program.
Then you need to calculate the P I and D terms.
P_correction = KP * error
I_correction = I_correction + ( KI * error)
D_correction = KD * (last_error - error) Note: This is a simple implementation, but often has problems due to noise in your sensor input. But in your case, since the error will change only in integer steps and the change will persist for only one program loop, this term won't do much of anything.
Then you can add them up to get the PID output.
Then you need to actually tune your controller to work.