Added very basic self-driving functions and general slowing down based on target velocity.  Not actually used in simulator yet