Added testing, sorting out why velocity seemed off