Finalized velocity test function

Replaced pseudo code with actual arduino commands to communicate with
motors and logic board.