Updated Autonomous Code

Added stop method, utilized mechanumdrive method