Basic Swerve Code for a Swerve Module using Victor SP motor controllers, an MA3 Absolute Encoder, and a navX Gyro
This code was ported from BaseFalconSwerve, but much of the original code was rewritten or removed.
Since this code was made for a specific unique robot, all the configuration that the original BaseFalconSwerve requires is already complete. I'm going to leave only the steps that will likely need to be done in the future. The actual steps may differ slightly because of the rewrite.

11. Setting Offsets

    • For finding the offsets, use a piece of 1x1 metal that is straight against the forks of the front and back modules (on the left and right side) to ensure that the modules are straight.
    • Point the bevel gears of all the wheels in the same direction (either facing left or right), where a postive input to the drive motor drives the robot forward (you can use phoenix tuner to test this). If for some reason you set the offsets with the wheels backwards, you can change the driveMotorInvert value to fix.
    • Open smartdashboard (or shuffleboard and go to the smartdashboard tab), you will see 4 printouts called "Mod 0 Cancoder", "Mod 1 Cancoder", etc.
      If you have already straightened the modules, copy those 4 numbers exactly (to 2 decimal places) to their respective angleOffset variable in constants.
      Note: The CANcoder values printed to smartdashboard are in degrees, when copying the values to angleOffset you must use Rotation2d.fromDegrees("copied value").

12. Angle Motor PID Values:
    If you are using a supported module, this value will be automatically set. If you are not, or prefer a more or less aggressive response, you can use the below instructions to tune.

    • To tune start with a low P value (0.01).
    • Multiply by 10 until the module starts oscilating around the set point
    • Scale back by searching for the value (for example, if it starts oscillating at a P of 10, then try (10 -> 5 -> 7.5 -> etc)) until the module doesn't oscillate around the setpoint.
    • If there is any overshoot you can add in some D by repeating the same process, leave at 0 if not. Always leave I at 0.

13. maxSpeed: In Meters Per Second. maxAngularVelocity: In Radians Per Second. For these you can use the theoretical values, but it is better to physically drive the robot and find the actual max values.

14. Get the drive characterization values (KS, KV, KA) by using the WPILib characterization tool, found here. You will need to lock your modules straight forward, and complete the characterization as if it was a standard tank drive.

15. driveKP:
    After completeing characterization and inserting the KS, KV, and KA values into the code, tune the drive motor kP until it doesn't overshoot and doesnt oscilate around a target velocity.
    Leave driveKI, driveKD, and driveKF at 0.0.

Controller Mappings

This code is natively setup to use a xbox controller to control the swerve drive.

• Left Stick: Translation Control (forwards and sideways movement)
• Right Stick: Rotation Control
  
• Y button: Zero Gyro (useful if the gyro drifts mid match, just rotate the robot forwards, and press Y to rezero)
• Left Bumper: Switches To Robot Centric Control while held