[Kingofl337]

I'll look into that issue tomorrow. I'll let you know.
The values your robot prints to screen should be current as of the last
time the variables were updated in the program loop.

Example of Current Loop:
X = X - Y;
printf("PrintX: %d ",X);

Example Loop Behind:
printf("PrintX: %d ",X);
X = X - Y;

You do have to take into account the fact there is a 26ms delay between loops.

If the change is large and quick alot can happen in a loop. If you have a 200 line encoder and your wheel is spinning at 300RPM there will be jumps in the
numbers of ticks between loops.