Added drive code

Added the main joystick to drive the robot, and added it to the rest of the code.