Swerve Implementation

[ZanyHunter]

Example:

public class RobotMap {
    public static final int FRONT_LEFT_DRIVE_MOTOR = 1;
    public static final int FRONT_LEFT_TURN_ENCODER = 0;
    public static final int FRONT_RIGHT_DRIVE_MOTOR = 3;
    public static final int FRONT_RIGHT_TURN_ENCODER = 2;
    public static final int BACK_LEFT_DRIVE_MOTOR = 5;
    public static final int BACK_LEFT_TURN_ENCODER = 4;
    public static final int BACK_RIGHT_DRIVE_MOTOR = 7;
    public static final int BACK_RIGHT_TURN_ENCODER = 6;
}

public class RobotContainer {
    private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(
        new Translation2d(0.5, 0.5),
        new Translation2d(0.5, -0.5),
        new Translation2d(-0.5, 0.5),
        new Translation2d(-0.5, -0.5)
    );

    private final SwerveDriveOdometry odometry = new SwerveDriveOdometry(kinematics, new Rotation2d());

    private final SwerveDrive swerveDrive = new SwerveDrive(
        new PWMVictorSPX(RobotMap.FRONT_LEFT_DRIVE_MOTOR),
        new AnalogEncoder(RobotMap.FRONT_LEFT_TURN_ENCODER),
        new PWMVictorSPX(RobotMap.FRONT_RIGHT_DRIVE_MOTOR),
        new AnalogEncoder(RobotMap.FRONT_RIGHT_TURN_ENCODER),
        new PWMVictorSPX(RobotMap.BACK_LEFT_DRIVE_MOTOR),
        new AnalogEncoder(RobotMap.BACK_LEFT_TURN_ENCODER),
        new PWMVictorSPX(RobotMap.BACK_RIGHT_DRIVE_MOTOR),
        new AnalogEncoder(RobotMap.BACK_RIGHT_TURN_ENCODER),
        kinematics::toSwerveModuleStates
    );

    public RobotContainer() {
        swerveDrive.setDefaultCommand(new RunCommand(() -> swerveDrive.drive(
            new ChassisSpeeds(0, 0, 0),
            kinematics.getModuleStates(
                new ChassisSpeeds(
                    0,
                    0,
                    0,
                    new Rotation2d()
                )
            )
        ), swerveDrive));
    }
}