This is the code we wrote in the summer of 2017 for our swerve drive robot (never yet used in competition). It uses a gyro to keep a fixed orientation relative to the field, and features (somewhat primitive) motion profiled autonomous capabilities.
This is a dual Eclipse and IntelliJ Idea project that your team is welcome to use for learning.
Though documentation is sparse, if you are also using Talon SRX's for both driving and turning, continous potentiometers for measuring rotation, and encoders on the drive wheels, and all these sensors are plugged directly into the corresponding Talon's, the teleop part of this demo should work for you.
For best results, edit k_wheelbaseLength and k_wheelbaseWidth to reflect your robot's measurements at the top of team2102.robot.subsystems.DriveSubsystem.