Merged with Lexie's Mecanum code, fixed some bugs.