Added test files to make life easier and updated autonomous drive method