Filled out some RobotDrive functions, getting Joystick ready