Updated code with velocity bug fix