Updating interpolation step after switching mode instead of after
enabling motor.