upgraded everything else to 2025 beta

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2"
+    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3"
     id "com.peterabeles.gversion" version "1.10"
     id("com.diffplug.spotless") version "7.0.0.BETA4"
 }

src/main/java/frc/robot/constants/TunerConstants.java

@@ -13,6 +13,7 @@
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
 import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType;
@@ -38,16 +39,17 @@ public class TunerConstants {
     // The steer motor uses any SwerveModule.SteerRequestType control request with the
     // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
     public static final Slot0Configs steerGains = new Slot0Configs()
-            .withKP(100)
+            .withKP(100.0)
             .withKI(0)
-            .withKD(2.0)
-            .withKS(0.2)
-            .withKV(1.5)
-            .withKA(0);
+            .withKD(1.0)
+            .withKS(0.08)
+            .withKV(2.66)
+            .withKA(0)
+            .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
     // When using closed-loop control, the drive motor uses the control
     // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
     public static final Slot0Configs driveGains =
-            new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.12);
+            new Slot0Configs().withKP(0.08).withKI(0).withKD(0).withKS(0.05).withKV(0.124);
 
     // The closed-loop output type to use for the steer motors;
     // This affects the PID/FF gains for the steer motors
@@ -77,23 +79,26 @@ public class TunerConstants {
     // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
     public static final Pigeon2Configuration pigeonConfigs = null;
 
+    // CAN bus that the devices are located on;
+    // All swerve devices must share the same CAN bus
+    public static final CANBus kCANBus = new CANBus("ChassisCanivore", "./logs/example.hoot");
+
     // Theoretical free speed (m/s) at 12 V applied output;
     // This needs to be tuned to your individual robot
-    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.70);
+    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73);
 
     // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
     // This may need to be tuned to your individual robot
-    public static final double kCoupleRatio = 3.5;
+    public static final double kCoupleRatio = 3.5714285714285716;
 
-    public static final double kDriveGearRatio = 7.363636364;
+    public static final double kDriveGearRatio = 5.36;
     public static final double kSteerGearRatio = 12.8;
-    public static final Distance kWheelRadius = Inches.of(2.167);
+    public static final Distance kWheelRadius = Inches.of(2);
 
     public static final boolean kInvertLeftSide = false;
     public static final boolean kInvertRightSide = true;
 
-    public static final CANBus kCANBus = new CANBus("rio", "./logs/example.hoot");
-    public static final int kPigeonId = 1;
+    public static final int kPigeonId = 0;
 
     // These are only used for simulation
     public static final double kSteerInertia = 0.01;
@@ -133,39 +138,43 @@ public class TunerConstants {
     public static final int kFrontLeftEncoderId = 1;
     public static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.305419921875);
     public static final boolean kFrontLeftSteerMotorInverted = true;
+    public static final boolean kFrontLeftCANcoderInverted = false;
 
-    public static final Distance kFrontLeftXPos = Inches.of(10.5);
-    public static final Distance kFrontLeftYPos = Inches.of(10.5);
+    public static final Distance kFrontLeftXPos = Inches.of(10);
+    public static final Distance kFrontLeftYPos = Inches.of(10);
 
     // Front Right
     public static final int kFrontRightDriveMotorId = 3;
     public static final int kFrontRightSteerMotorId = 4;
     public static final int kFrontRightEncoderId = 2;
     public static final Angle kFrontRightEncoderOffset = Rotations.of(-0.230712890625);
     public static final boolean kFrontRightSteerMotorInverted = true;
+    public static final boolean kFrontRightCANcoderInverted = false;
 
-    public static final Distance kFrontRightXPos = Inches.of(10.5);
-    public static final Distance kFrontRightYPos = Inches.of(-10.5);
+    public static final Distance kFrontRightXPos = Inches.of(10);
+    public static final Distance kFrontRightYPos = Inches.of(-10);
 
     // Back Left
     public static final int kBackLeftDriveMotorId = 5;
     public static final int kBackLeftSteerMotorId = 6;
     public static final int kBackLeftEncoderId = 3;
     public static final Angle kBackLeftEncoderOffset = Rotations.of(-0.221435546875);
     public static final boolean kBackLeftSteerMotorInverted = true;
+    public static final boolean kBackLeftCANcoderInverted = false;
 
-    public static final Distance kBackLeftXPos = Inches.of(-10.5);
-    public static final Distance kBackLeftYPos = Inches.of(10.5);
+    public static final Distance kBackLeftXPos = Inches.of(-10);
+    public static final Distance kBackLeftYPos = Inches.of(10);
 
     // Back Right
     public static final int kBackRightDriveMotorId = 7;
     public static final int kBackRightSteerMotorId = 8;
     public static final int kBackRightEncoderId = 4;
     public static final Angle kBackRightEncoderOffset = Rotations.of(-0.05419921875);
     public static final boolean kBackRightSteerMotorInverted = true;
+    public static final boolean kBackRightCANcoderInverted = false;
 
-    public static final Distance kBackRightXPos = Inches.of(-10.5);
-    public static final Distance kBackRightYPos = Inches.of(-10.5);
+    public static final Distance kBackRightXPos = Inches.of(-10);
+    public static final Distance kBackRightYPos = Inches.of(-10);
 
     public static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants(
             kFrontLeftSteerMotorId,
@@ -175,7 +184,8 @@ public class TunerConstants {
             kFrontLeftXPos,
             kFrontLeftYPos,
             kInvertLeftSide,
-            kFrontLeftSteerMotorInverted);
+            kFrontLeftSteerMotorInverted,
+            kFrontLeftCANcoderInverted);
     public static final SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants(
             kFrontRightSteerMotorId,
             kFrontRightDriveMotorId,
@@ -184,7 +194,8 @@ public class TunerConstants {
             kFrontRightXPos,
             kFrontRightYPos,
             kInvertRightSide,
-            kFrontRightSteerMotorInverted);
+            kFrontRightSteerMotorInverted,
+            kFrontRightCANcoderInverted);
     public static final SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants(
             kBackLeftSteerMotorId,
             kBackLeftDriveMotorId,
@@ -193,7 +204,8 @@ public class TunerConstants {
             kBackLeftXPos,
             kBackLeftYPos,
             kInvertLeftSide,
-            kBackLeftSteerMotorInverted);
+            kBackLeftSteerMotorInverted,
+            kBackLeftCANcoderInverted);
     public static final SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants(
             kBackRightSteerMotorId,
             kBackRightDriveMotorId,
@@ -202,7 +214,8 @@ public class TunerConstants {
             kBackRightXPos,
             kBackRightYPos,
             kInvertRightSide,
-            kBackRightSteerMotorInverted);
+            kBackRightSteerMotorInverted,
+            kBackRightCANcoderInverted);
 
     // These must be removed:
     //    public static CommandSwerveDrivetrain createDrivetrain() {

src/main/java/frc/robot/subsystems/drive/IO/ModuleIOTalon.java

@@ -14,6 +14,7 @@
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants;
@@ -54,12 +55,18 @@ public ModuleIOTalon(
         var driveConfig = moduleConstants.DriveMotorInitialConfigs;
         driveConfig.CurrentLimits.StatorCurrentLimit = DRIVE_CURRENT_LIMIT_ANTI_SLIP.in(Amps);
         driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+        driveConfig.MotorOutput.Inverted = moduleConstants.DriveMotorInverted
+                ? InvertedValue.Clockwise_Positive
+                : InvertedValue.CounterClockwise_Positive;
         driveTalon.getConfigurator().apply(driveConfig);
         setDriveBrake(true);
 
         var steerConfig = moduleConstants.SteerMotorInitialConfigs;
         steerConfig.CurrentLimits.StatorCurrentLimit = STEER_CURRENT_LIMIT.in(Amps);
         steerConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+        steerConfig.MotorOutput.Inverted = moduleConstants.SteerMotorInverted
+                ? InvertedValue.Clockwise_Positive
+                : InvertedValue.CounterClockwise_Positive;
         steerTalon.getConfigurator().apply(steerConfig);
         setSteerBrake(true);
 

src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVision.java

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.subsystems.MapleSubsystem;
 import frc.robot.subsystems.drive.HolonomicDriveSubsystem;
@@ -71,15 +72,10 @@ private Pose2d displayVisionPointEstimateResult(Optional<RobotPoseEstimationResu
     }
 
     private double getResultsTimeStamp() {
-        return inputs.inputsFetchedRealTimeStampSeconds - getResultsAverageLatencySeconds(inputs.camerasInputs);
-    }
-
-    private static double getResultsAverageLatencySeconds(AprilTagVisionIO.CameraInputs[] camerasInputs) {
-        if (camerasInputs.length == 0) return 0;
-        double totalLatencySeconds = 0;
-        for (AprilTagVisionIO.CameraInputs cameraInputs : camerasInputs)
-            totalLatencySeconds += cameraInputs.resultsDelaySeconds;
-
-        return totalLatencySeconds / camerasInputs.length;
+        if (inputs.camerasInputs.length == 0) return Timer.getTimestamp();
+        double totalTimeStampSeconds = 0;
+        for (AprilTagVisionIO.CameraInputs cameraInputs : inputs.camerasInputs)
+            totalTimeStampSeconds += cameraInputs.timeStampSeconds;
+        return totalTimeStampSeconds / inputs.camerasInputs.length;
     }
 }

src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVisionIO.java

@@ -13,7 +13,7 @@ public interface AprilTagVisionIO {
     class CameraInputs {
         public static final int MAX_TARGET_PER_CAMERA = 5;
         public boolean cameraConnected;
-        public double resultsDelaySeconds;
+        public double timeStampSeconds;
         public int currentTargetsCount;
         public final int[] fiducialMarksID;
         public final Transform3d[] bestCameraToTargets;
@@ -27,37 +27,37 @@ public CameraInputs() {
 
         public void clear() {
             this.cameraConnected = false;
-            this.resultsDelaySeconds = 0;
+            this.timeStampSeconds = 0;
             this.currentTargetsCount = 0;
             Arrays.fill(fiducialMarksID, -1);
             Arrays.fill(bestCameraToTargets, new Transform3d());
         }
 
         public void fromPhotonPipeLine(PhotonPipelineResult pipelineResult, boolean cameraConnected) {
             this.cameraConnected = cameraConnected;
-            this.resultsDelaySeconds = pipelineResult.getLatencyMillis() / 1000.0;
+            this.timeStampSeconds = pipelineResult.getTimestampSeconds();
             this.currentTargetsCount = Math.min(pipelineResult.getTargets().size(), MAX_TARGET_PER_CAMERA);
             Arrays.fill(fiducialMarksID, -1);
             Arrays.fill(bestCameraToTargets, new Transform3d());
             for (int i = 0; i < currentTargetsCount && i < MAX_TARGET_PER_CAMERA; i++) {
                 this.fiducialMarksID[i] = pipelineResult.getTargets().get(i).getFiducialId();
                 this.bestCameraToTargets[i] = pipelineResult.getTargets().get(i).getBestCameraToTarget();
             }
-            this.bestFieldToCamera = pipelineResult.getMultiTagResult().estimatedPose.isPresent
-                    ? Optional.of(pipelineResult.getMultiTagResult().estimatedPose.best)
+            this.bestFieldToCamera = pipelineResult.getMultiTagResult().isPresent()
+                    ? Optional.of(pipelineResult.getMultiTagResult().get().estimatedPose.best)
                     : Optional.empty();
         }
 
         private static final Transform3d NULL_TRANSFORM = new Transform3d(-114514, -114514, -114514, new Rotation3d());
 
         public void fromLog(LogTable table, int cameraID) {
-            final String cameraKey = "camera" + cameraID;
+            final String cameraKey = "Camera" + cameraID;
             this.cameraConnected = table.get(cameraKey + "Connected", false);
-            this.resultsDelaySeconds = table.get(cameraKey + "ResultsDelaySeconds", 0.0);
+            this.timeStampSeconds = table.get(cameraKey + "TimeStampSeconds", 0.0);
             this.currentTargetsCount = table.get(cameraKey + "CurrentTargetsCount", 0);
             final int[] fiducialMarkIDLogged = table.get(cameraKey + "FiducialMarksID", new int[MAX_TARGET_PER_CAMERA]);
             final Transform3d[] bestCameraToTargetsLogged =
-                    table.get(cameraKey + "bestCameraToTargets", new Transform3d[MAX_TARGET_PER_CAMERA]);
+                    table.get(cameraKey + "BestCameraToTargets", new Transform3d[MAX_TARGET_PER_CAMERA]);
             if (fiducialMarkIDLogged.length != MAX_TARGET_PER_CAMERA
                     || bestCameraToTargetsLogged.length != MAX_TARGET_PER_CAMERA)
                 DriverStation.reportError("vision log length not match", false);
@@ -66,20 +66,20 @@ public void fromLog(LogTable table, int cameraID) {
                 bestCameraToTargets[i] = bestCameraToTargetsLogged[i];
             }
 
-            if (table.get(cameraKey + "bestCameraToFieldPresents", false))
-                this.bestFieldToCamera = Optional.of(table.get(cameraKey + "bestCameraToField", new Transform3d()));
+            if (table.get(cameraKey + "BestCameraToFieldPresents", false))
+                this.bestFieldToCamera = Optional.of(table.get(cameraKey + "BestCameraToField", new Transform3d()));
             else this.bestFieldToCamera = Optional.empty();
         }
 
         public void writeToLog(LogTable table, int cameraID) {
-            final String cameraKey = "camera" + cameraID;
+            final String cameraKey = "Camera" + cameraID;
             table.put(cameraKey + "Connected", cameraConnected);
-            table.put(cameraKey + "ResultsDelaySeconds", resultsDelaySeconds);
+            table.put(cameraKey + "TimeStampSeconds", timeStampSeconds);
             table.put(cameraKey + "CurrentTargetsCount", currentTargetsCount);
             table.put(cameraKey + "FiducialMarksID", fiducialMarksID);
-            table.put(cameraKey + "bestCameraToTargets", bestCameraToTargets);
-            table.put(cameraKey + "bestCameraToFieldPresents", bestFieldToCamera.isPresent());
-            table.put(cameraKey + "bestCameraToField", bestFieldToCamera.orElse(new Transform3d()));
+            table.put(cameraKey + "BestCameraToTargets", bestCameraToTargets);
+            table.put(cameraKey + "BestCameraToFieldPresents", bestFieldToCamera.isPresent());
+            table.put(cameraKey + "BestCameraToField", bestFieldToCamera.orElse(new Transform3d()));
         }
     }
 

vendordeps/PathplannerLib-beta.json

@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib-beta.json",
     "name": "PathplannerLib",
-    "version": "2025.0.0-beta-5",
+    "version": "2025.0.0-beta-6.1",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,15 +12,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2025.0.0-beta-5"
+            "version": "2025.0.0-beta-6.1"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2025.0.0-beta-5",
+            "version": "2025.0.0-beta-6.1",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,