Swerve Drive with YAGSL

info

this will not go over the json configuration of yagsl

public class SwerveSubsystem extends SubsystemBase {
    private final SwerveDrive swerveDrive;

    public SwerveSubsystem(File dir, PhotonVision photonVision) {
        SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
        try {
            swerveDrive = new SwerveParser(dir).createSwerveDrive();
        } catch(Exception e) {
            throw new RuntimeException(e);
        }
    }

    public void drive(Translation2d translation, double rotation, boolean feildRelative, boolean isOpenLoop, boolean headingCorrection) {
        swerveDrive.drive(translation, rotation, feildRelative, isOpenLoop, headingCorrection);
    }
    public SwerveDriveKinematics getKinematics() {
        return swerveDrive.kinematics;
    }

    public void resetOdometry(Pose2d initialHolonomicPose) {
        setRotation(initialHolonomicPose.getRotation().getRadians());
        swerveDrive.resetOdometry(initialHolonomicPose);
    }

    public void setRotation(double rotation) {
        swerveDrive.setGyro(new Rotation3d(0, 0, rotation));
    }

    public Pose2d getPose() {
        return swerveDrive.getPose();
    }

    public void setChasisSpeeds(ChassisSpeeds speeds) {
        swerveDrive.setChassisSpeeds(speeds);
    }

    public void postTrajectory(Trajectory trajectory) {
        swerveDrive.postTrajectory(trajectory);
    }

    public void zeroGyro() {
        swerveDrive.zeroGyro();
    }

    public void setMotorBrake(boolean brake) {
        swerveDrive.setMotorIdleMode(brake);
    }

    public Rotation2d getHeading() {
        return swerveDrive.getYaw();
    }

    public Rotation2d getPitch() {
        return swerveDrive.getPitch();
    }

    public Rotation2d getRoll() {
        return swerveDrive.getRoll();
    }

    public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY) {
        xInput = Math.pow(xInput, 3);
        yInput = Math.pow(yInput, 3);
        return swerveDrive.swerveController.getTargetSpeeds(xInput, yInput, headingX, headingY);
    }

    public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) {
        xInput = Math.pow(xInput, 3);
        yInput = Math.pow(yInput, 3);
        return swerveDrive.swerveController.getTargetSpeeds(xInput, yInput, angle.getRadians(), getHeading().getRadians());
    }

    public ChassisSpeeds getFeildVelocity() {
        return swerveDrive.getFieldVelocity();
    }

    public ChassisSpeeds getRobotVelocity() {
        return swerveDrive.getRobotVelocity();
    }

    public SwerveController getSwerveController() {
        return swerveDrive.swerveController;
    }

    public SwerveDriveConfiguration getConfiguration() {
        return swerveDrive.swerveDriveConfiguration;
    }

    public void lockPose() {
        swerveDrive.lockPose();
    }

    public void addVisionMeasurement(Pose2d robotPose, double timeStamp, boolean soft, int trustWorthiness) {
        swerveDrive.addVisionMeasurement(robotPose, timeStamp, soft, trustWorthiness);
    }
}

public class TeleopDrive extends CommandBase {
    private final SwerveSubsystem swerve;
    private final DoubleSupplier vX;
    private final DoubleSupplier vY;
    private final DoubleSupplier omega;
    private final DoubleSupplier throttle;
    private final BooleanSupplier feildRelitive;
    private final boolean isOpenLoop;
    private final SwerveController controller;
    private final boolean headingCorrection;

    public TeleopDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier omega, DoubleSupplier throttle, BooleanSupplier feildRelitive, boolean isOpenLoop, boolean headingCorrection) {
        this.swerve = swerve;
        this.vX = vX;
        this.vY = vY;
        this.omega = omega;
        this.throttle = throttle;
        this.feildRelitive = feildRelitive;
        this.isOpenLoop = isOpenLoop;
        this.controller = swerve.getSwerveController();
        this.headingCorrection = headingCorrection;
        addRequirements(swerve);
    }


    @Override
    public void execute() {
        double modvX = vX.getAsDouble();
        double modvY = vY.getAsDouble();
        if(Math.abs(vX.getAsDouble()) < 0.2 && Math.abs(vY.getAsDouble()) < 0.2) {
            modvX = 0;
            modvY = 0;
        }

        double xVelocity = (modvX * controller.config.maxSpeed) * MathUtil.clamp(throttle.getAsDouble(), 0.1, 1);
        double yVelocity = (modvY * controller.config.maxSpeed) * MathUtil.clamp(throttle.getAsDouble(), 0.1, 1);
        double angVelocity = (Math.pow(omega.getAsDouble(), 3) * controller.config.maxAngularVelocity) * 0.5;
        swerve.drive(new Translation2d(xVelocity, yVelocity), angVelocity, feildRelitive.getAsBoolean(), isOpenLoop, headingCorrection);
    }
}

driverController = new CommandXboxController(0);

TeleopDrive teleopCommand = new TeleopDrive(
    swerve,
    () -> -driverController.getLeftX,
    () -> -driverController.getLeftY,
    () -> -driverController.getRightX,
    driverController::getRightTriggerAxis,
    () -> true,
    false,
    false
);