Major Change

Fixing motor speed problem. Twitch was taking position information
instead of speed information. Michelle contributed functions to
calculate speeds required to achieve linear motion of robot arm.
Initial VelocityTestFunc

pseudo code