Code Hygiene

src/main/java/frc/robot/Constants.java

@@ -45,6 +45,7 @@
 import frc.robot.util.RBSIEnum.SwerveType;
 import frc.robot.util.RBSIEnum.VisionType;
 import frc.robot.util.RobotDeviceId;
+import java.util.Set;
 import org.photonvision.simulation.SimCameraProperties;
 import swervelib.math.Matter;
 
@@ -126,19 +127,18 @@ public static final class RobotConstants {
     // Insert here the orientation (CCW == +) of the Rio and IMU from the robot
     // An angle of "0." means the x-y-z markings on the device match the robot's intrinsic reference
     //   frame.
-    // NOTE: It is assumed that both the Rio and the IMU are mounted such that +Z is UP
-    public static final Rotation2d kRioOrientation =
+    public static final Rotation3d kRioOrientation =
         switch (getRobot()) {
-          case COMPBOT -> Rotation2d.fromDegrees(-90.);
-          case DEVBOT -> Rotation2d.fromDegrees(0.);
-          default -> Rotation2d.fromDegrees(0.);
+          case COMPBOT -> new Rotation3d(0, 0, -90);
+          case DEVBOT -> Rotation3d.kZero;
+          default -> Rotation3d.kZero;
         };
     // IMU can be one of Pigeon2 or NavX
-    public static final Rotation2d kIMUOrientation =
+    public static final Rotation3d kIMUOrientation =
         switch (getRobot()) {
-          case COMPBOT -> Rotation2d.fromDegrees(0.);
-          case DEVBOT -> Rotation2d.fromDegrees(0.);
-          default -> Rotation2d.fromDegrees(0.);
+          case COMPBOT -> Rotation3d.kZero;
+          case DEVBOT -> Rotation3d.kZero;
+          default -> Rotation3d.kZero;
         };
   }
 
@@ -324,6 +324,9 @@ public static final class DrivebaseConstants {
         SwerveConstants.kDriveGearRatio / DCMotor.getKrakenX60Foc(1).KtNMPerAmp;
     public static final double kSteerP = 400.0;
     public static final double kSteerD = 20.0;
+
+    // Odometry-related constants
+    public static final double kHistorySize = 1.5; // seconds
   }
 
   /************************************************************************* */
@@ -416,8 +419,8 @@ public static final class AutoConstants {
   /** Vision Constants (Assuming PhotonVision) ***************************** */
   public static class VisionConstants {
 
-    public static final java.util.Set<Integer> kTrustedTags =
-        java.util.Set.of(2, 3, 4, 5, 8, 9, 10, 11, 18, 19, 20, 21, 24, 25, 26, 27); // HUB AprilTags
+    public static final Set<Integer> kTrustedTags =
+        Set.of(2, 3, 4, 5, 8, 9, 10, 11, 18, 19, 20, 21, 24, 25, 26, 27); // HUB AprilTags
 
     // Noise scaling factors (lower = more trusted)
     public static final double kTrustedTagStdDevScale = 0.6; // 40% more weight

src/main/java/frc/robot/RobotContainer.java

@@ -3,9 +3,15 @@
 // Copyright (c) 2021-2026 Littleton Robotics
 // http://github.com/Mechanical-Advantage
 //
-// Use of this source code is governed by a BSD
-// license that can be found in the AdvantageKit-License.md file
-// at the root directory of this project.
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
 // Copyright (c) FIRST and other WPILib contributors.
 // Open Source Software; you can modify and/or share it under the terms of
@@ -34,6 +40,7 @@
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.CANBuses;
+import frc.robot.Constants.Cameras;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.FieldConstants.AprilTagLayoutType;
 import frc.robot.commands.AutopilotCommands;
@@ -98,16 +105,14 @@ public class RobotContainer {
 
   // These are "Virtual Subsystems" that report information but have no motors
   private final Imu m_imu;
+  private final Vision m_vision;
 
   @SuppressWarnings("unused")
   private final Accelerometer m_accel;
 
   @SuppressWarnings("unused")
   private final RBSIPowerMonitor m_power;
 
-  @SuppressWarnings("unused")
-  private final Vision m_vision;
-
   @SuppressWarnings("unused")
   private List<RBSICANHealth> canHealth;
 
@@ -167,7 +172,7 @@ public RobotContainer() {
         m_flywheel = new Flywheel(new FlywheelIOSim());
 
         // ---------------- Vision IOs (robot code) ----------------
-        var cams = frc.robot.Constants.Cameras.ALL;
+        var cams = Cameras.ALL;
         m_vision =
             new Vision(
                 m_drivebase, m_drivebase::addVisionMeasurement, buildVisionIOsSim(m_drivebase));
@@ -520,12 +525,12 @@ private void definesysIdRoutines() {
   private VisionIO[] buildVisionIOsReal(Drive drive) {
     return switch (Constants.getVisionType()) {
       case PHOTON ->
-          java.util.Arrays.stream(Constants.Cameras.ALL)
+          Arrays.stream(Constants.Cameras.ALL)
               .map(c -> (VisionIO) new VisionIOPhotonVision(c.name(), c.robotToCamera()))
               .toArray(VisionIO[]::new);
 
       case LIMELIGHT ->
-          java.util.Arrays.stream(Constants.Cameras.ALL)
+          Arrays.stream(Constants.Cameras.ALL)
               .map(c -> (VisionIO) new VisionIOLimelight(c.name(), drive::getHeading))
               .toArray(VisionIO[]::new);
 

src/main/java/frc/robot/subsystems/accelerometer/Accelerometer.java

@@ -13,6 +13,7 @@
 
 package frc.robot.subsystems.accelerometer;
 
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.Timer;
 import frc.robot.Constants;
 import frc.robot.Constants.RobotConstants;
@@ -26,30 +27,20 @@
  * <p>This virtual subsystem pulls the acceleration values from both the RoboRIO and the swerve's
  * IMU (either Pigeon2 or NavX) and logs them to both AdvantageKitd. In addition to the
  * accelerations, the jerk (a-dot or x-tripple-dot) is computed from the delta accelerations.
- *
- * <p>Primitive-only hot path: no WPILib geometry objects or Units objects.
  */
 public class Accelerometer extends VirtualSubsystem {
+
+  // Gravitational acceleration
   private static final double G_TO_MPS2 = 9.80665;
 
+  // Define hardware interfaces
   private final RioAccelIO rio;
   private final RioAccelIO.Inputs rioInputs = new RioAccelIO.Inputs();
   private final Imu imu;
 
-  // Precompute yaw-only rotation terms
-  private static final double rioCos = Math.cos(RobotConstants.kRioOrientation.getRadians());
-  private static final double rioSin = Math.sin(RobotConstants.kRioOrientation.getRadians());
-  private static final double imuCos = Math.cos(RobotConstants.kIMUOrientation.getRadians());
-  private static final double imuSin = Math.sin(RobotConstants.kIMUOrientation.getRadians());
-
-  // Previous Rio accel (m/s^2)
-  private double prevRioAx = 0.0, prevRioAy = 0.0, prevRioAz = 0.0;
-
-  // Reusable arrays for logging
-  private final double[] rioAccelArr = new double[3];
-  private final double[] rioJerkArr = new double[3];
-  private final double[] imuAccelArr = new double[3];
-  private final double[] imuJerkArr = new double[3];
+  // Variables needed during the periodic
+  private Translation3d rawRio, rioAcc, rioJerk, imuAcc, imuJerk;
+  private Translation3d prevRioAcc = Translation3d.kZero;
 
   // Log decimation
   private int loopCount = 0;
@@ -74,58 +65,27 @@ public void rbsiPeriodic() {
     rio.updateInputs(rioInputs);
 
     // Compute RIO accelerations and jerks
-    final double rawX = rioInputs.xG;
-    final double rawY = rioInputs.yG;
-    final double rawZ = rioInputs.zG;
-
-    final double rioAx = (rioCos * rawX - rioSin * rawY) * G_TO_MPS2;
-    final double rioAy = (rioSin * rawX + rioCos * rawY) * G_TO_MPS2;
-    final double rioAz = rawZ * G_TO_MPS2;
-
-    final double rioJx = (rioAx - prevRioAx) / Constants.loopPeriodSecs;
-    final double rioJy = (rioAy - prevRioAy) / Constants.loopPeriodSecs;
-    final double rioJz = (rioAz - prevRioAz) / Constants.loopPeriodSecs;
+    rawRio = new Translation3d(rioInputs.xG, rioInputs.yG, rioInputs.zG);
+    rioAcc = rawRio.rotateBy(RobotConstants.kRioOrientation).times(G_TO_MPS2);
 
     // Acceleration from previous loop
-    prevRioAx = rioAx;
-    prevRioAy = rioAy;
-    prevRioAz = rioAz;
-
-    // IMU rotate is also compute-only (already primitives)
-    final double imuAx = (imuCos * imuInputs.linearAccelX - imuSin * imuInputs.linearAccelY);
-    final double imuAy = (imuSin * imuInputs.linearAccelX + imuCos * imuInputs.linearAccelY);
-    final double imuAz = imuInputs.linearAccelZ;
-
-    final double imuJx = (imuCos * imuInputs.jerkX - imuSin * imuInputs.jerkY);
-    final double imuJy = (imuSin * imuInputs.jerkX + imuCos * imuInputs.jerkY);
-    final double imuJz = imuInputs.jerkZ;
-
-    // Fill accel arrays (still math)
-    rioAccelArr[0] = rioAx;
-    rioAccelArr[1] = rioAy;
-    rioAccelArr[2] = rioAz;
-    imuAccelArr[0] = imuAx;
-    imuAccelArr[1] = imuAy;
-    imuAccelArr[2] = imuAz;
+    prevRioAcc = rioAcc;
 
-    final boolean doHeavyLogs = (++loopCount >= LOG_EVERY_N);
-    if (doHeavyLogs) {
-      loopCount = 0;
-      rioJerkArr[0] = rioJx;
-      rioJerkArr[1] = rioJy;
-      rioJerkArr[2] = rioJz;
-      imuJerkArr[0] = imuJx;
-      imuJerkArr[1] = imuJy;
-      imuJerkArr[2] = imuJz;
-    }
+    // IMU accelerations and jerks
+    imuAcc = imuInputs.linearAccel.rotateBy(RobotConstants.kIMUOrientation);
 
     // Logging
-    Logger.recordOutput("Accel/Rio/Accel_mps2", rioAccelArr);
-    Logger.recordOutput("Accel/IMU/Accel_mps2", imuAccelArr);
+    Logger.recordOutput("Accel/Rio/Accel_mps2", rioAcc);
+    Logger.recordOutput("Accel/IMU/Accel_mps2", imuAcc);
 
+    // Every N loops, compute and log the Jerk
+    final boolean doHeavyLogs = (++loopCount >= LOG_EVERY_N);
     if (doHeavyLogs) {
-      Logger.recordOutput("Accel/Rio/Jerk_mps3", rioJerkArr);
-      Logger.recordOutput("Accel/IMU/Jerk_mps3", imuJerkArr);
+      loopCount = 0;
+      rioJerk = rioAcc.minus(prevRioAcc).div(Constants.loopPeriodSecs);
+      imuJerk = imuInputs.linearJerk.rotateBy(RobotConstants.kIMUOrientation);
+      Logger.recordOutput("Accel/Rio/Jerk_mps3", rioJerk);
+      Logger.recordOutput("Accel/IMU/Jerk_mps3", imuJerk);
 
       final double[] ts = imuInputs.odometryYawTimestamps;
       if (ts.length > 0) {