Working Drive code

Working drive code with Dashboard printout and Autonomous diagnostic drive code.