Refactor Auto code to support new mecanum method