Added a bit of driving to autonomous

Testing and refinement is required