Added functions to DriveSubsystem and added a command to allow for driving.