Improved drive functions and mapped controller teleop correctly