Added drive method that sets the speed for both motors
Added Motor Command variables that are cleaner and easier to manipulate
General cleanup