PID fix & added timestep consideration.