Replace `SwerveSubsystem.java` and YAGSL with custom AK-compatible swerve by Scaratech · Pull Request #13 · FRC-3329/Atlas-II

Scaratech · 2026-04-17T12:00:42Z

This PR removes the previous swerve code and the YAGSL library with custom swerve code. This is a pretty big change and I'd like additional human input, so I'm making this a PR.

…ble swerve

harrislegobrick

First pass, but I'm not too sure how intentional deviation from the akit example is.
For example, the akit example has custom logic to run the odometry at a higher frequency than the robot code, but that is not implemented here. That functionality isn't strictly required, but since it is already implemented in the example code, it would be nice to have. Not sure if you planned on doing that in follow up PR or this one.

Overall looks fine, but some useful comments would be nice. The comments in the akit example are really good imo.

harrislegobrick · 2026-04-17T20:02:05Z

+        private static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) {
+            Rotation2d delta = desiredState.angle.minus(currentAngle);
+
+            if (Math.abs(delta.getDegrees()) > 90.0) {


the SwerveModuleState class has a method to do the optimization logic for you with the .optimize(Rotation2d) method.

harrislegobrick · 2026-04-17T20:02:12Z

+        return Math.copySign(deadbanded * deadbanded, deadbanded);
+    }
+
+    private static boolean shouldFlipForAlliance() {


This is broken out into its own file in the akit example. I am fine with keeping it here if you'd want to keep it here, but I'd rather it be in a separate file in the module directory.

harrislegobrick · 2026-04-17T20:02:20Z

+
+    private static double modifyAxis(double value) {
+        double deadbanded = Math.abs(value) > OperatorConstants.DEADBAND ? value : 0.0;
+        return Math.copySign(deadbanded * deadbanded, deadbanded);


WPILib also has a function for this too with MathUtil.copydirectionPow(double, double, double).

harrislegobrick · 2026-04-17T20:02:24Z

+    }
+
+    private static double modifyAxis(double value) {
+        double deadbanded = Math.abs(value) > OperatorConstants.DEADBAND ? value : 0.0;


WPILib has a function for doing this with MathUtil.applyDeadband(double, double).

harrislegobrick · 2026-04-17T20:03:09Z

+            double omega;
+            if (aim.getAsBoolean()) {
+                Translation2d toTarget = aimTarget.get().getTranslation().minus(getPose().getTranslation());
+                Rotation2d targetHeading = toTarget.getAngle().plus(Rotation2d.fromDegrees(180.0));


Rotation2d.fromDegrees(180.0) can be replaced with Rotation2d.k180deg. The description for the pre-made rotation values say that using them can reduce memory usage by sharing the commonly used rotation values.

harrislegobrick · 2026-04-17T20:03:20Z

+        SparkUtil.tryUntilOk(
+                driveMotor,
+                5,
+                () -> driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters,


I think this would overwrite the configs set earlier.

harrislegobrick · 2026-04-17T20:03:40Z

+
+    @Override
+    public void setDesiredState(SwerveModuleState state) {
+        if (Math.abs(state.speedMetersPerSecond) < 0.01) {


I wouldn't have slow velocity filtering in the module but rather in the input into the module. Also the state should be optimized with both the optimizations in the akit example.

harrislegobrick · 2026-04-17T20:03:47Z

+        encoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5;
+        absoluteEncoder.getConfigurator().apply(encoderConfig);
+
+        SparkMaxConfig driveConfig = new SparkMaxConfig();


Should also set the signals for CAN bus optimization.

harrislegobrick · 2026-04-17T20:03:55Z

+        driveMotor = new SparkMax(driveId, MotorType.kBrushless);
+        angleMotor = new SparkMax(angleId, MotorType.kBrushless);
+
+        absoluteEncoder = new CANcoder(encoderId);


Not calling the optimize CAN method.

harrislegobrick · 2026-04-17T20:04:15Z

+import frc.robot.constants.Constants;
+import frc.robot.constants.SwerveConstants;
+
+public class ModuleIOSim implements ModuleIO {


This doesn't have the DCMotorSim for actual sim support, so this class wouldn't really do anything right now.

harrislegobrick

Looks pretty good, just needs a few changes.

harrislegobrick · 2026-04-24T20:51:41Z

+        linearMagnitude = linearMagnitude * linearMagnitude;
+
+        // Return new linear velocity
+        return new Pose2d(Translation2d.kZero, linearDirection)


This can be replaced by return new Translation2d(linearMagnitude, linearDirection);

harrislegobrick · 2026-04-24T20:52:02Z

+                this,
+                translationX,
+                translationY,
+                () -> targetPoseSupplier.get().getTranslation().minus(getPose().getTranslation()).getAngle());


Would be nice to also supply an offset to be added to the calculated angle considering the turret's angle isn't zero. I'd also probably change from accepting a Supplier<Pose2d> to just a Supplier<Translation2d>.

harrislegobrick · 2026-04-24T20:52:12Z

+        return Commands.either(targetLockDrive, manualDrive, aimAtTarget).withName("DriveCommand");
+    }
+
+    /** Convenience overload for WPILib triggers. */


The Trigger class implements BooleanSupplier so this isn't actually needed.

harrislegobrick · 2026-04-24T20:52:18Z

+    private final int index;
+
+    private final Alert driveDisconnectedAlert;
+    private final Alert turnDisconnectedAlert;


Should probably have one for the CANCoder too.

harrislegobrick · 2026-04-24T20:52:25Z

+
+public interface ModuleIO {
+    @AutoLog
+    public static class ModuleIOInputs {


In the example project, the absolute encoder is assumed to be plugged into the SparkMAX directly so there is no accessing it differently, but for our setup, we have a CANCoder for the absolute angle. I think the information about the absolute encoder (mainly connection and position) should be logged separately from the turn motor's information.

harrislegobrick · 2026-04-24T20:52:36Z

+
+        // Create odometry queues
+        timestampQueue = SparkOdometryThread.getInstance().makeTimestampQueue();
+        drivePositionQueue = SparkOdometryThread.getInstance().registerSignal(driveSpark, driveEncoder::getPosition);


This should use a copied status signal like the gyro.

harrislegobrick · 2026-04-24T20:52:42Z

+                        PersistMode.kNoPersistParameters));
+    }
+
+    private void syncTurnEncoderWithAbsolute() {


YAGSL syncs the absolute encoder with the motor's internal encoder every 100ms as long as the wheel isn't turning/moving. I'm not sure how necessary that is, but it is a similar idea to the turret where it uses the absolute information to correct the internal encoder after some time. I don't think the swerve turn will mechanically slip so I don't think it is necessary, but is something to be aware of and is why I want both logged.

harrislegobrick · 2026-04-28T00:42:25Z

+                translationX,
+                translationY,
+                () -> targetTranslationSupplier.get().minus(getPose().getTranslation()).getAngle().plus(angleOffset));
+        return Commands.either(targetLockDrive, manualDrive, aimAtTarget).withName("DriveCommand");


This won't work like the current system does because of how the Command.either() works. It is a ConditionalCommand where it chooses between the two commands only once when the command is initialized, and since this is the default command for the drivetrain, this will only call the initialization once at the start of teleop.

harrislegobrick · 2026-04-28T00:42:47Z

+    }
+
+    @Override
+    public void updateInputs(ModuleIOInputs inputs) {


Should also set the two new IO variables added for the absolute CANcoder. It can probably just use the value produced in the sim from the turn position along with always being connected.

harrislegobrick

Probably fine for an initial implementation.

harrislegobrick · 2026-04-28T19:31:28Z

+                new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION));
+
+        angleController.enableContinuousInput(-Math.PI, Math.PI);
+        boolean[] wasUsingAngle = new boolean[] { false };


Why is this an array?

feat: replace SwerveSubsystem.java and YAGSL with custom AK-compata…

e3cb4d7

…ble swerve

Scaratech requested a review from harrislegobrick April 17, 2026 12:00

Scaratech changed the title ~~Replace SwerveSubsystem.java and YAGSL with custom compatible swerve~~ Replace SwerveSubsystem.java and YAGSL with custom AK-compatible swerve Apr 17, 2026

harrislegobrick suggested changes Apr 17, 2026

View reviewed changes

Scaratech added 2 commits April 24, 2026 08:03

feat: refactor entire thing (steal example project)

bcbc703

feat: constants + spark max

faa7984

Scaratech requested a review from harrislegobrick April 24, 2026 12:09

harrislegobrick suggested changes Apr 24, 2026

View reviewed changes

fix: requested changes

983ef3f

Scaratech requested a review from harrislegobrick April 27, 2026 11:33

harrislegobrick suggested changes Apr 28, 2026

View reviewed changes

fix: things

c7469bf

harrislegobrick approved these changes Apr 28, 2026

View reviewed changes

Uh oh!

Conversation

Scaratech commented Apr 17, 2026

Uh oh!

harrislegobrick left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

harrislegobrick left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

harrislegobrick left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants